FreeCalypso > hg > freecalypso-sw
annotate rvinterf/etm/pktsort.c @ 184:4714fdfca39c
fc-tmsh compiles!
author | Michael Spacefalcon <msokolov@ivan.Harhan.ORG> |
---|---|
date | Sun, 24 Nov 2013 08:40:44 +0000 |
parents | 85222710dc92 |
children | 2f285f20d617 |
rev | line source |
---|---|
183
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
1 /* |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
2 * Here we sort out incoming packets from the target relayed via rvinterf. |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
3 */ |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
4 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
5 #include <sys/types.h> |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
6 #include <stdio.h> |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
7 #include <string.h> |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
8 #include <strings.h> |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
9 #include <stdlib.h> |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
10 #include "../pktmux.h" |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
11 #include "../limits.h" |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
12 #include "../localsock.h" |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
13 #include "localtypes.h" |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
14 #include "etm.h" |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
15 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
16 extern u_char rvi_msg[]; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
17 extern int rvi_msg_len; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
18 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
19 void |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
20 safe_print_trace(src, srclen, dest) |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
21 u_char *src; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
22 char *dest; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
23 { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
24 int i, c; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
25 char *dp; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
26 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
27 dp = dest; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
28 for (i = 0; i < srclen; i++) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
29 c = src[i]; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
30 if (c & 0x80) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
31 *dp++ = 'M'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
32 *dp++ = '-'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
33 c &= 0x7F; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
34 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
35 if (c < 0x20) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
36 *dp++ = '^'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
37 *dp++ = c + '@'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
38 } else if (c == 0x7F) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
39 *dp++ = '^'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
40 *dp++ = '?'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
41 } else |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
42 *dp++ = c; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
43 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
44 *dp = '\0'; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
45 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
46 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
47 static void |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
48 handle_useid_0() |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
49 { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
50 char buf[MAX_PKT_FROM_TARGET*4]; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
51 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
52 if (strncmp(rvi_msg + 7, "RVT: Lost Message", 17)) |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
53 return; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
54 safe_print_trace(rvi_msg + 7, rvi_msg_len - 7, buf); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
55 async_msg_output(buf); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
56 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
57 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
58 static void |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
59 print_etm_trace() |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
60 { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
61 char buf[MAX_PKT_FROM_TARGET*4]; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
62 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
63 strcpy(buf, "ETM Tr: "); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
64 safe_print_trace(rvi_msg + 7, rvi_msg_len - 7, buf + 9); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
65 async_msg_output(buf); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
66 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
67 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
68 static void |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
69 process_rvt() |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
70 { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
71 u32 useid; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
72 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
73 if (rvi_msg_len < 7) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
74 tty_cleanup(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
75 fprintf(stderr, "Error: rvinterf sent us an invalid RVT msg\n"); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
76 exit(1); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
77 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
78 useid = rvi_msg[2] << 24 | rvi_msg[3] << 16 | rvi_msg[4] << 8 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
79 | rvi_msg[5]; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
80 switch (useid) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
81 case 0: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
82 handle_useid_0(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
83 return; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
84 case ETM_USE_ID: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
85 print_etm_trace(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
86 return; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
87 default: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
88 tty_cleanup(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
89 fprintf(stderr, "unexpected fwd of USEID %08X from rvinterf\n", |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
90 useid); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
91 exit(1); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
92 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
93 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
94 |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
95 void |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
96 process_pkt_from_target() |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
97 { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
98 switch (rvi_msg[1]) { |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
99 case RVT_RV_HEADER: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
100 process_rvt(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
101 return; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
102 case RVT_TM_HEADER: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
103 etm_packet_rx(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
104 return; |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
105 default: |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
106 tty_cleanup(); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
107 fprintf(stderr, "unexpected fwd of MUX %02X from rvinterf\n", |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
108 rvi_msg[1]); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
109 exit(1); |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
110 } |
85222710dc92
rvinterf/etm: implemented everything except the packet sending command
Michael Spacefalcon <msokolov@ivan.Harhan.ORG>
parents:
diff
changeset
|
111 } |