1 // SPDX-License-Identifier: ISC 2 /* 3 * Copyright (c) 2010 Broadcom Corporation 4 */ 5 6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt 7 8 #include <linux/netdevice.h> 9 #include <linux/module.h> 10 11 #include <brcmu_utils.h> 12 13 MODULE_AUTHOR("Broadcom Corporation"); 14 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities."); 15 MODULE_LICENSE("Dual BSD/GPL"); 16 17 struct sk_buff *brcmu_pkt_buf_get_skb(uint len) 18 { 19 struct sk_buff *skb; 20 21 skb = dev_alloc_skb(len); 22 if (skb) { 23 skb_put(skb, len); 24 skb->priority = 0; 25 } 26 27 return skb; 28 } 29 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb); 30 31 /* Free the driver packet. Free the tag if present */ 32 void brcmu_pkt_buf_free_skb(struct sk_buff *skb) 33 { 34 if (!skb) 35 return; 36 37 WARN_ON(skb->next); 38 dev_kfree_skb_any(skb); 39 } 40 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb); 41 42 /* 43 * osl multiple-precedence packet queue 44 * hi_prec is always >= the number of the highest non-empty precedence 45 */ 46 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec, 47 struct sk_buff *p) 48 { 49 struct sk_buff_head *q; 50 51 if (pktq_full(pq) || pktq_pfull(pq, prec)) 52 return NULL; 53 54 q = &pq->q[prec].skblist; 55 skb_queue_tail(q, p); 56 pq->len++; 57 58 if (pq->hi_prec < prec) 59 pq->hi_prec = (u8) prec; 60 61 return p; 62 } 63 EXPORT_SYMBOL(brcmu_pktq_penq); 64 65 struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec, 66 struct sk_buff *p) 67 { 68 struct sk_buff_head *q; 69 70 if (pktq_full(pq) || pktq_pfull(pq, prec)) 71 return NULL; 72 73 q = &pq->q[prec].skblist; 74 skb_queue_head(q, p); 75 pq->len++; 76 77 if (pq->hi_prec < prec) 78 pq->hi_prec = (u8) prec; 79 80 return p; 81 } 82 EXPORT_SYMBOL(brcmu_pktq_penq_head); 83 84 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec) 85 { 86 struct sk_buff_head *q; 87 struct sk_buff *p; 88 89 q = &pq->q[prec].skblist; 90 p = skb_dequeue(q); 91 if (p == NULL) 92 return NULL; 93 94 pq->len--; 95 return p; 96 } 97 EXPORT_SYMBOL(brcmu_pktq_pdeq); 98 99 /* 100 * precedence based dequeue with match function. Passing a NULL pointer 101 * for the match function parameter is considered to be a wildcard so 102 * any packet on the queue is returned. In that case it is no different 103 * from brcmu_pktq_pdeq() above. 104 */ 105 struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec, 106 bool (*match_fn)(struct sk_buff *skb, 107 void *arg), void *arg) 108 { 109 struct sk_buff_head *q; 110 struct sk_buff *p, *next; 111 112 q = &pq->q[prec].skblist; 113 skb_queue_walk_safe(q, p, next) { 114 if (match_fn == NULL || match_fn(p, arg)) { 115 skb_unlink(p, q); 116 pq->len--; 117 return p; 118 } 119 } 120 return NULL; 121 } 122 EXPORT_SYMBOL(brcmu_pktq_pdeq_match); 123 124 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec) 125 { 126 struct sk_buff_head *q; 127 struct sk_buff *p; 128 129 q = &pq->q[prec].skblist; 130 p = skb_dequeue_tail(q); 131 if (p == NULL) 132 return NULL; 133 134 pq->len--; 135 return p; 136 } 137 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail); 138 139 void 140 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir, 141 bool (*fn)(struct sk_buff *, void *), void *arg) 142 { 143 struct sk_buff_head *q; 144 struct sk_buff *p, *next; 145 146 q = &pq->q[prec].skblist; 147 skb_queue_walk_safe(q, p, next) { 148 if (fn == NULL || (*fn) (p, arg)) { 149 skb_unlink(p, q); 150 brcmu_pkt_buf_free_skb(p); 151 pq->len--; 152 } 153 } 154 } 155 EXPORT_SYMBOL(brcmu_pktq_pflush); 156 157 void brcmu_pktq_flush(struct pktq *pq, bool dir, 158 bool (*fn)(struct sk_buff *, void *), void *arg) 159 { 160 int prec; 161 for (prec = 0; prec < pq->num_prec; prec++) 162 brcmu_pktq_pflush(pq, prec, dir, fn, arg); 163 } 164 EXPORT_SYMBOL(brcmu_pktq_flush); 165 166 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len) 167 { 168 int prec; 169 170 /* pq is variable size; only zero out what's requested */ 171 memset(pq, 0, 172 offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec)); 173 174 pq->num_prec = (u16) num_prec; 175 176 pq->max = (u16) max_len; 177 178 for (prec = 0; prec < num_prec; prec++) { 179 pq->q[prec].max = pq->max; 180 skb_queue_head_init(&pq->q[prec].skblist); 181 } 182 } 183 EXPORT_SYMBOL(brcmu_pktq_init); 184 185 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out) 186 { 187 int prec; 188 189 if (pq->len == 0) 190 return NULL; 191 192 for (prec = 0; prec < pq->hi_prec; prec++) 193 if (!skb_queue_empty(&pq->q[prec].skblist)) 194 break; 195 196 if (prec_out) 197 *prec_out = prec; 198 199 return skb_peek_tail(&pq->q[prec].skblist); 200 } 201 EXPORT_SYMBOL(brcmu_pktq_peek_tail); 202 203 /* Return sum of lengths of a specific set of precedences */ 204 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp) 205 { 206 int prec, len; 207 208 len = 0; 209 210 for (prec = 0; prec <= pq->hi_prec; prec++) 211 if (prec_bmp & (1 << prec)) 212 len += pq->q[prec].skblist.qlen; 213 214 return len; 215 } 216 EXPORT_SYMBOL(brcmu_pktq_mlen); 217 218 /* Priority dequeue from a specific set of precedences */ 219 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp, 220 int *prec_out) 221 { 222 struct sk_buff_head *q; 223 struct sk_buff *p; 224 int prec; 225 226 if (pq->len == 0) 227 return NULL; 228 229 while ((prec = pq->hi_prec) > 0 && 230 skb_queue_empty(&pq->q[prec].skblist)) 231 pq->hi_prec--; 232 233 while ((prec_bmp & (1 << prec)) == 0 || 234 skb_queue_empty(&pq->q[prec].skblist)) 235 if (prec-- == 0) 236 return NULL; 237 238 q = &pq->q[prec].skblist; 239 p = skb_dequeue(q); 240 if (p == NULL) 241 return NULL; 242 243 pq->len--; 244 245 if (prec_out) 246 *prec_out = prec; 247 248 return p; 249 } 250 EXPORT_SYMBOL(brcmu_pktq_mdeq); 251 252 /* Produce a human-readable string for boardrev */ 253 char *brcmu_boardrev_str(u32 brev, char *buf) 254 { 255 char c; 256 257 if (brev < 0x100) { 258 snprintf(buf, BRCMU_BOARDREV_LEN, "%d.%d", 259 (brev & 0xf0) >> 4, brev & 0xf); 260 } else { 261 c = (brev & 0xf000) == 0x1000 ? 'P' : 'A'; 262 snprintf(buf, BRCMU_BOARDREV_LEN, "%c%03x", c, brev & 0xfff); 263 } 264 return buf; 265 } 266 EXPORT_SYMBOL(brcmu_boardrev_str); 267 268 char *brcmu_dotrev_str(u32 dotrev, char *buf) 269 { 270 u8 dotval[4]; 271 272 if (!dotrev) { 273 snprintf(buf, BRCMU_DOTREV_LEN, "unknown"); 274 return buf; 275 } 276 dotval[0] = (dotrev >> 24) & 0xFF; 277 dotval[1] = (dotrev >> 16) & 0xFF; 278 dotval[2] = (dotrev >> 8) & 0xFF; 279 dotval[3] = dotrev & 0xFF; 280 281 if (dotval[3]) 282 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d.%d.%d", dotval[0], 283 dotval[1], dotval[2], dotval[3]); 284 else if (dotval[2]) 285 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d.%d", dotval[0], 286 dotval[1], dotval[2]); 287 else 288 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d", dotval[0], 289 dotval[1]); 290 291 return buf; 292 } 293 EXPORT_SYMBOL(brcmu_dotrev_str); 294 295 #if defined(DEBUG) 296 /* pretty hex print a pkt buffer chain */ 297 void brcmu_prpkt(const char *msg, struct sk_buff *p0) 298 { 299 struct sk_buff *p; 300 301 if (msg && (msg[0] != '\0')) 302 pr_debug("%s:\n", msg); 303 304 for (p = p0; p; p = p->next) 305 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len); 306 } 307 EXPORT_SYMBOL(brcmu_prpkt); 308 309 void brcmu_dbg_hex_dump(const void *data, size_t size, const char *fmt, ...) 310 { 311 struct va_format vaf; 312 va_list args; 313 314 va_start(args, fmt); 315 316 vaf.fmt = fmt; 317 vaf.va = &args; 318 319 pr_debug("%pV", &vaf); 320 321 va_end(args); 322 323 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size); 324 } 325 EXPORT_SYMBOL(brcmu_dbg_hex_dump); 326 327 #endif /* defined(DEBUG) */ 328