1 /*
2 * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
3 *
4 * Permission to use, copy, modify, and/or distribute this software for
5 * any purpose with or without fee is hereby granted, provided that the
6 * above copyright notice and this permission notice appear in all
7 * copies.
8 *
9 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
10 * WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
11 * WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
12 * AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
13 * DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
14 * PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
15 * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
16 * PERFORMANCE OF THIS SOFTWARE.
17 */
18
19 #include <qdf_pkt_add_timestamp.h>
20
21 static struct dp_pkt_add_ts_info dp_pkt_ts_info;
22 static int dp_pkt_add_timestamp;
23
qdf_set_dp_pkt_add_ts_info(enum qdf_pkt_supported_proto proto,uint16_t port,uint16_t offset)24 int qdf_set_dp_pkt_add_ts_info(enum qdf_pkt_supported_proto proto,
25 uint16_t port, uint16_t offset)
26 {
27 if (dp_pkt_ts_info.current_index >= NUM_DP_PKT_TIMESTAMP_SUPPORT) {
28 qdf_err("Only %d number of protocol supported",
29 NUM_DP_PKT_TIMESTAMP_SUPPORT);
30 return -EINVAL;
31 }
32
33 dp_pkt_ts_info.enable_protocol_bitmap |= (1 << proto);
34 dp_pkt_ts_info.proto_info[dp_pkt_ts_info.current_index].proto = proto;
35 dp_pkt_ts_info.proto_info[dp_pkt_ts_info.current_index].port = port;
36 dp_pkt_ts_info.proto_info[dp_pkt_ts_info.current_index++].offset =
37 offset;
38 dp_pkt_add_timestamp = 1;
39 return 0;
40 }
41
qdf_clear_dp_pkt_add_ts_info(void)42 void qdf_clear_dp_pkt_add_ts_info(void)
43 {
44 dp_pkt_add_timestamp = 0;
45 qdf_mem_zero(&dp_pkt_ts_info, sizeof(dp_pkt_ts_info));
46 }
47
48 static
qdf_get_proto_str(enum qdf_pkt_supported_proto proto)49 const char *qdf_get_proto_str(enum qdf_pkt_supported_proto proto)
50 {
51 switch (proto) {
52 case QDF_PKT_PROTO_TCP:
53 return "TCP";
54 case QDF_PKT_PROTO_UDP:
55 return "UDP";
56 default:
57 return "Invalid";
58 }
59 }
60
qdf_show_dp_pkt_add_ts_info(char * buf,size_t size)61 int qdf_show_dp_pkt_add_ts_info(char *buf, size_t size)
62 {
63 int i;
64 int cnt = 0;
65
66 qdf_debug("dp_pkt_add_timestamp %d", dp_pkt_add_timestamp);
67 qdf_debug("enable_protocol_bitmap %x",
68 dp_pkt_ts_info.enable_protocol_bitmap);
69 qdf_debug("current_index %d", dp_pkt_ts_info.current_index);
70
71 cnt += scnprintf(buf + cnt, size - cnt, "dp_pkt_add_timestamp %d\n",
72 dp_pkt_add_timestamp);
73 for (i = 0; i < dp_pkt_ts_info.current_index; i++) {
74 qdf_debug("proto %d port %d offset %d",
75 dp_pkt_ts_info.proto_info[i].proto,
76 dp_pkt_ts_info.proto_info[i].port,
77 dp_pkt_ts_info.proto_info[i].offset);
78 if (size - cnt <= 0)
79 continue;
80 cnt += scnprintf(buf + cnt, size - cnt,
81 "Protocol: %s Destination Port %d Offset %d\n",
82 qdf_get_proto_str(
83 dp_pkt_ts_info.proto_info[i].proto),
84 dp_pkt_ts_info.proto_info[i].port,
85 dp_pkt_ts_info.proto_info[i].offset);
86 }
87 return cnt;
88 }
89
qdf_is_dp_pkt_timestamp_enabled(void)90 bool qdf_is_dp_pkt_timestamp_enabled(void)
91 {
92 return dp_pkt_add_timestamp;
93 }
94
95 static inline
qdf_get_tcp_offset(qdf_nbuf_t nbuf,uint16_t offset)96 uint32_t qdf_get_tcp_offset(qdf_nbuf_t nbuf, uint16_t offset)
97 {
98 uint16_t ip_header_len, tcp_header_len, tcp_header_off;
99 uint8_t *skb_data = (uint8_t *)qdf_nbuf_data(nbuf);
100
101 ip_header_len = ((uint8_t)(*(uint8_t *)
102 (skb_data + QDF_NBUF_TRAC_IPV4_OFFSET)) &
103 QDF_NBUF_TRAC_IPV4_HEADER_MASK) << 2;
104 tcp_header_off = QDF_NBUF_TRAC_IPV4_OFFSET + ip_header_len;
105 tcp_header_len = ((uint8_t)(*(uint8_t *)
106 (skb_data + tcp_header_off +
107 QDF_NBUF_TRAC_TCP_HEADER_LEN_OFFSET))) >> 2;
108 return tcp_header_off + tcp_header_len + offset;
109 }
110
111 static inline
qdf_get_udp_offset(qdf_nbuf_t nbuf,uint16_t offset)112 uint32_t qdf_get_udp_offset(qdf_nbuf_t nbuf, uint16_t offset)
113 {
114 uint16_t ip_header_len, udp_header_len;
115 uint8_t *skb_data = (uint8_t *)qdf_nbuf_data(nbuf);
116
117 ip_header_len = ((uint8_t)(*(uint8_t *)
118 (skb_data + QDF_NBUF_TRAC_IPV4_OFFSET)) &
119 QDF_NBUF_TRAC_IPV4_HEADER_MASK) << 2;
120 udp_header_len = 8;
121 return QDF_NBUF_TRAC_IPV4_OFFSET + ip_header_len +
122 udp_header_len + offset;
123 }
124
125 static inline
qdf_add_ts(qdf_nbuf_t nbuf,uint32_t offset,enum qdf_pkt_timestamp_index index,uint64_t time,enum qdf_pkt_supported_proto proto)126 void qdf_add_ts(qdf_nbuf_t nbuf, uint32_t offset,
127 enum qdf_pkt_timestamp_index index, uint64_t time,
128 enum qdf_pkt_supported_proto proto)
129 {
130 struct ts *ts_ptr;
131 struct ts_info *ts_info;
132 uint32_t total_offset;
133
134 if (proto == QDF_PKT_PROTO_TCP)
135 total_offset = qdf_get_tcp_offset(nbuf, offset);
136 else if (proto == QDF_PKT_PROTO_UDP)
137 total_offset = qdf_get_udp_offset(nbuf, offset);
138 else
139 return;
140
141 if (qdf_nbuf_len(nbuf) < total_offset + sizeof(struct ts))
142 return;
143
144 ts_ptr = (struct ts *)(qdf_nbuf_data(nbuf) + total_offset);
145 ts_info = &ts_ptr->ts_info[index];
146
147 ts_info->sec = time / 1000000;
148 ts_info->usec = time % 1000000;
149 }
150
qdf_add_dp_pkt_timestamp(qdf_nbuf_t nbuf,enum qdf_pkt_timestamp_index index,uint64_t time)151 void qdf_add_dp_pkt_timestamp(qdf_nbuf_t nbuf,
152 enum qdf_pkt_timestamp_index index, uint64_t time)
153 {
154 int i;
155 uint16_t port;
156 uint32_t offset;
157
158 if (dp_pkt_ts_info.enable_protocol_bitmap & QDF_PKT_PROTO_TCP_BIT) {
159 if (qdf_nbuf_is_ipv4_tcp_pkt(nbuf)) {
160 port =
161 QDF_SWAP_U16(qdf_nbuf_data_get_tcp_dst_port(nbuf));
162 for (i = 0; i < dp_pkt_ts_info.current_index; i++) {
163 offset = dp_pkt_ts_info.proto_info[i].offset;
164 if (dp_pkt_ts_info.proto_info[i].proto ==
165 QDF_PKT_PROTO_TCP &&
166 dp_pkt_ts_info.proto_info[i].port == port) {
167 qdf_add_ts(nbuf, offset, index, time,
168 QDF_PKT_PROTO_TCP);
169 break;
170 }
171 }
172 return;
173 }
174 }
175
176 if (dp_pkt_ts_info.enable_protocol_bitmap & QDF_PKT_PROTO_UDP_BIT) {
177 if (qdf_nbuf_is_ipv4_udp_pkt(nbuf)) {
178 port =
179 QDF_SWAP_U16(qdf_nbuf_data_get_tcp_dst_port(nbuf));
180 for (i = 0; i < dp_pkt_ts_info.current_index; i++) {
181 offset = dp_pkt_ts_info.proto_info[i].offset;
182 if (dp_pkt_ts_info.proto_info[i].proto ==
183 QDF_PKT_PROTO_UDP &&
184 dp_pkt_ts_info.proto_info[i].port == port) {
185 qdf_add_ts(nbuf, offset, index, time,
186 QDF_PKT_PROTO_UDP);
187 break;
188 }
189 }
190 return;
191 }
192 }
193 }
194