1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2004-2007 Marcel Holtmann <marcel (at) 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 #include <sys/types.h> 34 35 #include "parser.h" 36 37 #define PPP_U8(frm) (get_u8(frm)) 38 #define PPP_U16(frm) (btohs(htons(get_u16(frm)))) 39 #define PPP_U32(frm) (btohl(htonl(get_u32(frm)))) 40 41 static int ppp_traffic = 0; 42 43 static unsigned char ppp_magic1[] = { 0x7e, 0xff, 0x03, 0xc0, 0x21 }; 44 static unsigned char ppp_magic2[] = { 0x7e, 0xff, 0x7d, 0x23, 0xc0, 0x21 }; 45 static unsigned char ppp_magic3[] = { 0x7e, 0x7d, 0xdf, 0x7d, 0x23, 0xc0, 0x21 }; 46 47 static inline int check_for_ppp_traffic(unsigned char *data, int size) 48 { 49 int i; 50 51 for (i = 0; i < size - sizeof(ppp_magic1); i++) 52 if (!memcmp(data + i, ppp_magic1, sizeof(ppp_magic1))) { 53 ppp_traffic = 1; 54 return i; 55 } 56 57 for (i = 0; i < size - sizeof(ppp_magic2); i++) 58 if (!memcmp(data + i, ppp_magic2, sizeof(ppp_magic2))) { 59 ppp_traffic = 1; 60 return i; 61 } 62 63 for (i = 0; i < size - sizeof(ppp_magic3); i++) 64 if (!memcmp(data + i, ppp_magic3, sizeof(ppp_magic3))) { 65 ppp_traffic = 1; 66 return i; 67 } 68 69 return -1; 70 } 71 72 static inline char *dir2str(uint8_t in) 73 { 74 return in ? "DCE" : "DTE"; 75 } 76 77 static inline char *proto2str(uint16_t proto) 78 { 79 switch (proto) { 80 case 0x0001: 81 return "Padding Protocol"; 82 case 0x0021: 83 return "IP"; 84 case 0x8021: 85 return "IP Control Protocol"; 86 case 0x80fd: 87 return "Compression Control Protocol"; 88 case 0xc021: 89 return "Link Control Protocol"; 90 case 0xc023: 91 return "Password Authentication Protocol"; 92 case 0xc025: 93 return "Link Quality Report"; 94 case 0xc223: 95 return "Challenge Handshake Authentication Protocol"; 96 default: 97 return "Unknown Protocol"; 98 } 99 } 100 101 static void hdlc_dump(int level, struct frame *frm) 102 { 103 uint8_t addr = get_u8(frm); 104 uint8_t ctrl = get_u8(frm); 105 uint16_t fcs, proto; 106 107 fcs = bt_get_unaligned((uint16_t *) (frm->ptr + frm->len - 2)); 108 frm->len -= 2; 109 110 p_indent(level, frm); 111 112 if (addr != 0xff || ctrl != 0x03) { 113 frm->ptr -= 2; 114 frm->len += 2; 115 printf("HDLC: %s: len %d fcs 0x%04x\n", 116 dir2str(frm->in), frm->len, fcs); 117 } else 118 printf("HDLC: %s: addr 0x%02x ctrl 0x%02x len %d fcs 0x%04x\n", 119 dir2str(frm->in), addr, ctrl, frm->len, fcs); 120 121 if (*((uint8_t *) frm->ptr) & 0x80) 122 proto = get_u16(frm); 123 else 124 proto = get_u8(frm); 125 126 p_indent(level + 1, frm); 127 printf("PPP: %s (0x%04x): len %d\n", proto2str(proto), proto, frm->len); 128 129 raw_dump(level + 1, frm); 130 } 131 132 static inline void unslip_frame(int level, struct frame *frm, int len) 133 { 134 struct frame msg; 135 unsigned char *data, *ptr; 136 int i, p = 0; 137 138 data = malloc(len * 2); 139 if (!data) 140 return; 141 142 ptr = frm->ptr; 143 144 for (i = 0; i < len; i++) { 145 if (ptr[i] == 0x7d) { 146 data[p++] = ptr[i + 1] ^ 0x20; 147 i++; 148 } else 149 data[p++] = ptr[i]; 150 } 151 152 memset(&msg, 0, sizeof(msg)); 153 msg.data = data; 154 msg.data_len = len * 2; 155 msg.ptr = msg.data; 156 msg.len = p; 157 msg.in = frm->in; 158 msg.ts = frm->ts; 159 msg.handle = frm->handle; 160 msg.cid = frm->cid; 161 162 hdlc_dump(level, &msg); 163 164 free(data); 165 } 166 167 void ppp_dump(int level, struct frame *frm) 168 { 169 void *ptr, *end; 170 int err, len, pos = 0; 171 172 if (frm->pppdump_fd > fileno(stderr)) { 173 unsigned char id; 174 uint16_t len = htons(frm->len); 175 uint32_t ts = htonl(frm->ts.tv_sec & 0xffffffff); 176 177 id = 0x07; 178 err = write(frm->pppdump_fd, &id, 1); 179 err = write(frm->pppdump_fd, &ts, 4); 180 181 id = frm->in ? 0x02 : 0x01; 182 err = write(frm->pppdump_fd, &id, 1); 183 err = write(frm->pppdump_fd, &len, 2); 184 err = write(frm->pppdump_fd, frm->ptr, frm->len); 185 } 186 187 if (!ppp_traffic) { 188 pos = check_for_ppp_traffic(frm->ptr, frm->len); 189 if (pos < 0) { 190 raw_dump(level, frm); 191 return; 192 } 193 194 if (pos > 0) { 195 raw_ndump(level, frm, pos); 196 frm->ptr += pos; 197 frm->len -= pos; 198 } 199 } 200 201 frm = add_frame(frm); 202 203 while (frm->len > 0) { 204 ptr = memchr(frm->ptr, 0x7e, frm->len); 205 if (!ptr) 206 break; 207 208 if (frm->ptr != ptr) { 209 frm->len -= (ptr - frm->ptr); 210 frm->ptr = ptr; 211 } 212 213 end = memchr(frm->ptr + 1, 0x7e, frm->len - 1); 214 if (!end) 215 break; 216 217 len = end - ptr - 1; 218 219 frm->ptr++; 220 frm->len--; 221 222 if (len > 0) { 223 unslip_frame(level, frm, len); 224 225 frm->ptr += len; 226 frm->len -= len; 227 } 228 } 229 } 230
