xpp: README: hwid attribute of the xpd sysfs node
[dahdi/tools.git] / dahdi_pcap.c
1 /*
2  * Capturing a pcap from the DAHDI interface
3  * 
4  * Copyright (C) 2011 Torrey Searle
5  * 
6  * ISDN support added by Horacio Peña 
7  * Command line cleanups by Sverker Abrahamsson
8  * 
9  * Requirements:
10  * - pcap development library
11  * - DAHDI_MIRROR ioctl which isn't enabled by default in dahdi-linux
12  *   To enable this unsupported feature, #define CONFIG_DAHDI_MIRROR
13  *   in dahdi-linux
14  * - To build this program call the 'make dahdi_pcap' target
15  */
16
17 /*
18  * See http://www.asterisk.org for more information about
19  * the Asterisk project. Please do not directly contact
20  * any of the maintainers of this project for assistance;
21  * the project provides a web site, mailing lists and IRC
22  * channels for your use.
23  *
24  * This program is free software, distributed under the terms of
25  * the GNU General Public License Version 2 as published by the
26  * Free Software Foundation. See the LICENSE file included with
27  * this program for more details.
28  */
29
30 #include <stdio.h>
31 #include <fcntl.h>
32 #include <dahdi/user.h>
33 #include <sys/time.h>
34 #include <time.h>
35 #include <sys/types.h>
36 #include <unistd.h>
37 #include <string.h>
38 #include <pcap.h>
39 #include <sys/types.h>
40 #include <arpa/inet.h>
41 #include <sys/ioctl.h>
42 #include <stdlib.h>
43 #include <getopt.h>
44
45 #define BLOCK_SIZE 512
46 #define MAX_CHAN 16
47 //char ETH_P_LAPD[2] = {0x00, 0x30};
48
49 struct mtp2_phdr {
50         u_int8_t  sent;
51         u_int8_t  annex_a_used;
52         u_int16_t link_number;
53 };
54
55
56 struct lapd_sll_hdr {
57     u_int16_t sll_pkttype;    /* packet type */
58     u_int16_t sll_hatype;
59     u_int16_t sll_halen;
60     u_int8_t sll_addr[8];
61     u_int8_t sll_protocol[2];   /* protocol, should be ETH_P_LAPD */
62 };
63
64
65 struct chan_fds {
66         int rfd;
67         int tfd;
68         int chan_id;
69         int proto;
70         char tx_buf[BLOCK_SIZE * 4];
71         int tx_len;
72         char rx_buf[BLOCK_SIZE * 4];
73         int rx_len;
74 };
75
76 int make_mirror(long type, int chan)
77 {
78         int res = 0;
79         int fd = 0;     
80         struct dahdi_bufferinfo bi;
81         fd = open("/dev/dahdi/pseudo", O_RDONLY);
82
83         memset(&bi, 0, sizeof(bi));
84         bi.txbufpolicy = DAHDI_POLICY_IMMEDIATE;
85         bi.rxbufpolicy = DAHDI_POLICY_IMMEDIATE;
86         bi.numbufs = 32;
87         bi.bufsize = BLOCK_SIZE;
88
89         ioctl(fd, DAHDI_SET_BUFINFO, &bi);
90
91         res = ioctl(fd, type, &chan);
92
93         if(res)
94         {
95                 printf("error setting channel err=%d!\n", res);
96                 return -1;
97         }
98
99         
100         return fd;
101 }
102
103 int log_packet(struct chan_fds * fd, char is_read, pcap_dumper_t * dump)
104 {
105         unsigned char buf[BLOCK_SIZE * 4];
106         int res = 0;
107
108         struct pcap_pkthdr hdr;
109         struct mtp2_phdr * mtp2 = (struct mtp2_phdr *)buf;
110         struct lapd_sll_hdr * lapd = (struct lapd_sll_hdr *)buf;
111
112         unsigned char *dataptr = buf;
113         int datasize = sizeof(buf);
114
115         if(fd->proto == DLT_LINUX_LAPD)
116         {
117                 dataptr += sizeof(struct lapd_sll_hdr);
118                 datasize -= sizeof(struct lapd_sll_hdr);
119         }
120         else
121         {
122                 dataptr += sizeof(struct mtp2_phdr);
123                 datasize -= sizeof(struct mtp2_phdr);
124         }
125
126         memset(buf, 0, sizeof(buf));
127         if(is_read)
128         {
129                 res = read(fd->rfd, dataptr, datasize);
130                 if(fd->rx_len > 0 && res == fd->rx_len && !memcmp(fd->rx_buf, dataptr, res) )
131                 {
132                         //skipping dup
133                         return 0;
134                 }
135
136                 memcpy(fd->rx_buf,  dataptr, res);
137                 fd->rx_len = res;
138         }
139         else
140         {
141                 res = read(fd->tfd, dataptr, datasize);
142                 if(fd->tx_len > 0 && res == fd->tx_len && !memcmp(fd->tx_buf, dataptr, res) )
143                 {
144                         //skipping dup
145                         return 0;
146                 }
147
148                 memcpy(fd->tx_buf,  dataptr, res);
149                 fd->tx_len = res;
150         }
151
152         gettimeofday(&hdr.ts, NULL);
153
154         
155
156         
157         if(res > 0)
158         {
159                 if(fd->proto == DLT_LINUX_LAPD)
160                 {
161                         hdr.caplen = res+sizeof(struct lapd_sll_hdr)-2;
162                         hdr.len = res+sizeof(struct lapd_sll_hdr)-2;
163                         
164                         lapd->sll_pkttype = 3;
165                         lapd->sll_hatype = 0;
166                         lapd->sll_halen = res;
167         //                lapd->sll_addr = ???
168                         lapd->sll_protocol[0] = 0x00;
169                         lapd->sll_protocol[1] = 0x30;
170
171                 }
172                 else
173                 {
174                         hdr.caplen = res+sizeof(struct mtp2_phdr);
175                         hdr.len = res+sizeof(struct mtp2_phdr);
176                         
177                         if(is_read)
178                         {
179                                 mtp2->sent = 0;
180                                 mtp2->annex_a_used = 0;
181                         }
182                         else
183                         {
184                                 mtp2->sent = 1;
185                                 mtp2->annex_a_used = 0;
186                         }
187                         mtp2->link_number = htons(fd->chan_id);
188                 }
189                 pcap_dump((u_char*)dump, &hdr, buf);
190                 pcap_dump_flush(dump);
191         }
192         return 1;
193 }
194
195 void usage() 
196 {
197         printf("Usage: dahdi_pcap [OPTIONS]\n");
198         printf("Capture packets from DAHDI channels to pcap file\n\n");
199         printf("Options:\n");
200         printf("  -p, --proto=[mtp2|lapd] The protocol to capture, default mtp2\n");
201         printf("  -c, --chan=<channels>   Comma separated list of channels to capture from, max %d. Mandatory\n", MAX_CHAN);
202         printf("  -f, --file=<filename>   The pcap file to capture to. Mandatory\n");
203         printf("  -h, --help              Display this text\n");
204 }
205
206 int main(int argc, char **argv)
207 {
208         struct chan_fds chans[MAX_CHAN];
209         char *filename = NULL;
210         int num_chans = 0;
211         int max_fd = 0;
212         int proto = DLT_MTP2_WITH_PHDR;
213
214         int i;
215         int packetcount;
216         int c;
217
218         while (1) {
219                 int option_index = 0;
220                 static struct option long_options[] = {
221                         {"proto", required_argument, 0, 'p'},
222                         {"chan", required_argument, 0, 'c'},
223                         {"file", required_argument, 0, 'f'},
224                         {"help", 0, 0, 'h'},
225                         {0, 0, 0, 0}
226                 };
227
228                 c = getopt_long(argc, argv, "p:c:f:?",
229                           long_options, &option_index);
230                 if (c == -1)
231                         break;
232
233                 switch (c) {
234                         case 'p':
235                                 // Protocol
236                                 if(strcasecmp("LAPD", optarg)==0)
237                                 {
238                                         proto = DLT_LINUX_LAPD;
239                                 }
240                                 else if(argc > 0 && strcasecmp("MTP2", argv[1])==0)
241                                 {
242                                         proto = DLT_MTP2_WITH_PHDR;
243                                 }
244                                 break;
245                         case 'c':
246                                 // TODO Should it be possible to override protocol per channel?
247                                 // Channels, comma separated list
248                                 while(optarg != NULL && num_chans < MAX_CHAN)
249                                 {
250                                         int chan = atoi(strsep(&optarg, ","));
251
252
253                                         chans[num_chans].tfd = make_mirror(DAHDI_TXMIRROR, chan);
254                                         chans[num_chans].rfd = make_mirror(DAHDI_RXMIRROR, chan);
255                                         chans[num_chans].chan_id = chan;
256                                         chans[num_chans].proto = proto;
257
258                                         if(chans[num_chans].tfd > max_fd)
259                                         {
260                                                 max_fd = chans[num_chans].tfd;
261                                         }
262                                         if(chans[num_chans].rfd > max_fd)
263                                         {
264                                                 max_fd = chans[num_chans].rfd;
265                                         }
266
267                                         num_chans++;
268                                 }
269                                 max_fd++;
270                                 break;
271                         case 'f':
272                                 // File to capture to
273                                 filename=optarg;
274                                 break;
275                         case 'h':
276                         default:
277                                 // Usage
278                                 usage();
279                                 exit(0);
280                   }
281         }
282         if((num_chans == 0) || (filename == NULL)) {
283                 usage();
284                 exit(0);
285         }
286         else
287         {
288                 printf("Capturing protocol %s on channels ", (proto == DLT_MTP2_WITH_PHDR ? "mtp2":"lapd"));
289                 for(i = 0; i < num_chans; i++)
290                 {
291                         printf("%d", chans[i].chan_id);
292                         if(i<num_chans-1)
293                         {
294                                 printf(", ");
295                         }
296                 }
297                 printf(" to file %s\n", filename);
298         }
299
300         pcap_t * pcap = pcap_open_dead(chans[0].proto, BLOCK_SIZE*4);
301         pcap_dumper_t * dump = pcap_dump_open(pcap, filename);
302         
303         packetcount=0;
304         while(1)
305         {
306                 fd_set rd_set;
307                 FD_ZERO(&rd_set);
308                 for(i = 0; i < num_chans; i++)
309                 {
310                         FD_SET(chans[i].tfd, &rd_set);
311                         FD_SET(chans[i].rfd, &rd_set);
312                 }
313
314                 select(max_fd, &rd_set, NULL, NULL, NULL);
315
316                 for(i = 0; i < num_chans; i++)
317                 {
318                         if(FD_ISSET(chans[i].rfd, &rd_set))
319                         {
320                                 packetcount += log_packet(&chans[i], 1, dump);
321                         }
322                         if(FD_ISSET(chans[i].tfd, &rd_set))
323                         {
324                                 packetcount += log_packet(&chans[i], 0, dump);
325                         }
326                 }
327                 printf("Packets captured: %d\r", packetcount);
328                 fflush(stdout);
329         }
330
331         return 0;
332 }