1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2004-2007 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 #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
check_for_ppp_traffic(unsigned char * data,int size)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
dir2str(uint8_t in)72 static inline char *dir2str(uint8_t in)
73 {
74 return in ? "DCE" : "DTE";
75 }
76
proto2str(uint16_t proto)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
hdlc_dump(int level,struct frame * frm)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
unslip_frame(int level,struct frame * frm,int len)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
ppp_dump(int level,struct frame * frm)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