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 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 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 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 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 90 bool qdf_is_dp_pkt_timestamp_enabled(void) 91 { 92 return dp_pkt_add_timestamp; 93 } 94 95 static inline 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 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 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 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