tools/parser/cmtp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2002-2011  Marcel Holtmann <marcel@holtmann.org>
   6  *
   7  *
   8  *  This program is free software; you can redistribute it and/or modify
   9  *  it under the terms of the GNU General Public License as published by
  10  *  the Free Software Foundation; either version 2 of the License, or
  11  *  (at your option) any later version.
  12  *
  13  *  This program is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  *  GNU General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU General Public License
  19  *  along with this program; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <errno.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33
  34 #include "parser.h"
  35
  36 #define TABLE_SIZE 10
  37
  38 static struct {
  39         uint16_t handle;
  40         uint16_t cid;
  41         struct frame msg[16];
  42 } table[TABLE_SIZE];
  43
  44 static void add_segment(uint8_t bid, struct frame *frm, int len)
  45 {
  46         uint16_t handle = frm->handle, cid = frm->cid;
  47         struct frame *msg;
  48         void *data;
  49         int i, pos = -1;
  50
  51         if (bid > 15)
  52                 return;
  53
  54         for (i = 0; i < TABLE_SIZE; i++) {
  55                 if (table[i].handle == handle && table[i].cid == cid) {
  56                         pos = i;
  57                         break;
  58                 }
  59
  60                 if (pos < 0 && !table[i].handle && !table[i].cid)
  61                         pos = i;
  62         }
  63
  64         if (pos < 0)
  65                 return;
  66
  67         table[pos].handle = handle;
  68         table[pos].cid    = cid;
  69         msg = &table[pos].msg[bid];
  70
  71         data = malloc(msg->data_len + len);
  72         if (!data)
  73                 return;
  74
  75         if (msg->data_len > 0)
  76                 memcpy(data, msg->data, msg->data_len);
  77
  78         memcpy(data + msg->data_len, frm->ptr, len);
  79         free(msg->data);
  80         msg->data = data;
  81         msg->data_len += len;
  82         msg->ptr = msg->data;
  83         msg->len = msg->data_len;
  84         msg->in  = frm->in;
  85         msg->ts  = frm->ts;
  86         msg->handle = handle;
  87         msg->cid    = cid;
  88 }
  89
  90 static void free_segment(uint8_t bid, struct frame *frm)
  91 {
  92         uint16_t handle = frm->handle, cid = frm->cid;
  93         struct frame *msg;
  94         int i, len = 0, pos = -1;
  95
  96         if (bid > 15)
  97                 return;
  98
  99         for (i = 0; i < TABLE_SIZE; i++)
 100                 if (table[i].handle == handle && table[i].cid == cid) {
 101                         pos = i;
 102                         break;
 103                 }
 104
 105         if (pos < 0)
 106                 return;
 107
 108         msg = &table[pos].msg[bid];
 109
 110         if (msg->data)
 111                 free(msg->data);
 112
 113         msg->data = NULL;
 114         msg->data_len = 0;
 115
 116         for (i = 0; i < 16; i++)
 117                 len += table[pos].msg[i].data_len;
 118
 119         if (!len) {
 120                 table[pos].handle = 0;
 121                 table[pos].cid = 0;
 122         }
 123 }
 124
 125 static struct frame *get_segment(uint8_t bid, struct frame *frm)
 126 {
 127         uint16_t handle = frm->handle, cid = frm->cid;
 128         int i;
 129
 130         if (bid > 15)
 131                 return NULL;
 132
 133         for (i = 0; i < TABLE_SIZE; i++)
 134                 if (table[i].handle == handle && table[i].cid == cid)
 135                         return &table[i].msg[bid];
 136
 137         return NULL;
 138 }
 139
 140 static char *bst2str(uint8_t bst)
 141 {
 142         switch (bst) {
 143         case 0x00:
 144                 return "complete CAPI Message";
 145         case 0x01:
 146                 return "segmented CAPI Message";
 147         case 0x02:
 148                 return "error";
 149         case 0x03:
 150                 return "reserved";
 151         default:
 152                 return "unknown";
 153         }
 154 }
 155
 156 void cmtp_dump(int level, struct frame *frm)
 157 {
 158         struct frame *msg;
 159         uint8_t hdr, bid;
 160         uint16_t len;
 161
 162         while (frm->len > 0) {
 163
 164                 hdr = get_u8(frm);
 165                 bid = (hdr & 0x3c) >> 2;
 166
 167                 switch ((hdr & 0xc0) >> 6) {
 168                 case 0x01:
 169                         len = get_u8(frm);
 170                         break;
 171                 case 0x02:
 172                         len = htons(get_u16(frm));
 173                         break;
 174                 default:
 175                         len = 0;
 176                         break;
 177                 }
 178
 179                 p_indent(level, frm);
 180
 181                 printf("CMTP: %s: id %d len %d\n", bst2str(hdr & 0x03), bid, len);
 182
 183                 switch (hdr & 0x03) {
 184                 case 0x00:
 185                         add_segment(bid, frm, len);
 186                         msg = get_segment(bid, frm);
 187                         if (!msg)
 188                                 break;
 189
 190                         if (!p_filter(FILT_CAPI))
 191                                 capi_dump(level + 1, msg);
 192                         else
 193                                 raw_dump(level, msg);
 194
 195                         free_segment(bid, frm);
 196                         break;
 197                 case 0x01:
 198                         add_segment(bid, frm, len);
 199                         break;
 200                 default:
 201                         free_segment(bid, frm);
 202                         break;
 203                 }
 204
 205                 frm->ptr += len;
 206                 frm->len -= len;
 207         }
 208 }