[deliverable/linux.git] / drivers / net / wireless / brcm80211 / brcmutil / utils.c

/*
 * Copyright (c) 2010 Broadcom Corporation
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt

#include <linux/netdevice.h>
#include <linux/module.h>

#include <brcmu_utils.h>

MODULE_AUTHOR("Broadcom Corporation");
MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
MODULE_LICENSE("Dual BSD/GPL");

struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
{
	struct sk_buff *skb;

	skb = dev_alloc_skb(len);
	if (skb) {
		skb_put(skb, len);
		skb->priority = 0;
	}

	return skb;
}
EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);

/* Free the driver packet. Free the tag if present */
void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
{
	if (!skb)
		return;

	WARN_ON(skb->next);
	dev_kfree_skb_any(skb);
}
EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);

/*
 * osl multiple-precedence packet queue
 * hi_prec is always >= the number of the highest non-empty precedence
 */
struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
				      struct sk_buff *p)
{
	struct sk_buff_head *q;

	if (pktq_full(pq) || pktq_pfull(pq, prec))
		return NULL;

	q = &pq->q[prec].skblist;
	skb_queue_tail(q, p);
	pq->len++;

	if (pq->hi_prec < prec)
		pq->hi_prec = (u8) prec;

	return p;
}
EXPORT_SYMBOL(brcmu_pktq_penq);

struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
					   struct sk_buff *p)
{
	struct sk_buff_head *q;

	if (pktq_full(pq) || pktq_pfull(pq, prec))
		return NULL;

	q = &pq->q[prec].skblist;
	skb_queue_head(q, p);
	pq->len++;

	if (pq->hi_prec < prec)
		pq->hi_prec = (u8) prec;

	return p;
}
EXPORT_SYMBOL(brcmu_pktq_penq_head);

struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
{
	struct sk_buff_head *q;
	struct sk_buff *p;

	q = &pq->q[prec].skblist;
	p = skb_dequeue(q);
	if (p == NULL)
		return NULL;

	pq->len--;
	return p;
}
EXPORT_SYMBOL(brcmu_pktq_pdeq);

/*
 * precedence based dequeue with match function. Passing a NULL pointer
 * for the match function parameter is considered to be a wildcard so
 * any packet on the queue is returned. In that case it is no different
 * from brcmu_pktq_pdeq() above.
 */
struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec,
				      bool (*match_fn)(struct sk_buff *skb,
						       void *arg), void *arg)
{
	struct sk_buff_head *q;
	struct sk_buff *p, *next;

	q = &pq->q[prec].skblist;
	skb_queue_walk_safe(q, p, next) {
		if (match_fn == NULL || match_fn(p, arg)) {
			skb_unlink(p, q);
			pq->len--;
			return p;
		}
	}
	return NULL;
}
EXPORT_SYMBOL(brcmu_pktq_pdeq_match);

struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
{
	struct sk_buff_head *q;
	struct sk_buff *p;

	q = &pq->q[prec].skblist;
	p = skb_dequeue_tail(q);
	if (p == NULL)
		return NULL;

	pq->len--;
	return p;
}
EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);

void
brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
		  bool (*fn)(struct sk_buff *, void *), void *arg)
{
	struct sk_buff_head *q;
	struct sk_buff *p, *next;

	q = &pq->q[prec].skblist;
	skb_queue_walk_safe(q, p, next) {
		if (fn == NULL || (*fn) (p, arg)) {
			skb_unlink(p, q);
			brcmu_pkt_buf_free_skb(p);
			pq->len--;
		}
	}
}
EXPORT_SYMBOL(brcmu_pktq_pflush);

void brcmu_pktq_flush(struct pktq *pq, bool dir,
		      bool (*fn)(struct sk_buff *, void *), void *arg)
{
	int prec;
	for (prec = 0; prec < pq->num_prec; prec++)
		brcmu_pktq_pflush(pq, prec, dir, fn, arg);
}
EXPORT_SYMBOL(brcmu_pktq_flush);

void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
{
	int prec;

	/* pq is variable size; only zero out what's requested */
	memset(pq, 0,
	      offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));

	pq->num_prec = (u16) num_prec;

	pq->max = (u16) max_len;

	for (prec = 0; prec < num_prec; prec++) {
		pq->q[prec].max = pq->max;
		skb_queue_head_init(&pq->q[prec].skblist);
	}
}
EXPORT_SYMBOL(brcmu_pktq_init);

struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
{
	int prec;

	if (pq->len == 0)
		return NULL;

	for (prec = 0; prec < pq->hi_prec; prec++)
		if (!skb_queue_empty(&pq->q[prec].skblist))
			break;

	if (prec_out)
		*prec_out = prec;

	return skb_peek_tail(&pq->q[prec].skblist);
}
EXPORT_SYMBOL(brcmu_pktq_peek_tail);

/* Return sum of lengths of a specific set of precedences */
int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
{
	int prec, len;

	len = 0;

	for (prec = 0; prec <= pq->hi_prec; prec++)
		if (prec_bmp & (1 << prec))
			len += pq->q[prec].skblist.qlen;

	return len;
}
EXPORT_SYMBOL(brcmu_pktq_mlen);

/* Priority dequeue from a specific set of precedences */
struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
				      int *prec_out)
{
	struct sk_buff_head *q;
	struct sk_buff *p;
	int prec;

	if (pq->len == 0)
		return NULL;

	while ((prec = pq->hi_prec) > 0 &&
	       skb_queue_empty(&pq->q[prec].skblist))
		pq->hi_prec--;

	while ((prec_bmp & (1 << prec)) == 0 ||
	       skb_queue_empty(&pq->q[prec].skblist))
		if (prec-- == 0)
			return NULL;

	q = &pq->q[prec].skblist;
	p = skb_dequeue(q);
	if (p == NULL)
		return NULL;

	pq->len--;

	if (prec_out)
		*prec_out = prec;

	return p;
}
EXPORT_SYMBOL(brcmu_pktq_mdeq);

#if defined(DEBUG)
/* pretty hex print a pkt buffer chain */
void brcmu_prpkt(const char *msg, struct sk_buff *p0)
{
	struct sk_buff *p;

	if (msg && (msg[0] != '\0'))
		pr_debug("%s:\n", msg);

	for (p = p0; p; p = p->next)
		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
}
EXPORT_SYMBOL(brcmu_prpkt);

void brcmu_dbg_hex_dump(const void *data, size_t size, const char *fmt, ...)
{
	struct va_format vaf;
	va_list args;

	va_start(args, fmt);

	vaf.fmt = fmt;
	vaf.va = &args;

	pr_debug("%pV", &vaf);

	va_end(args);

	print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size);
}
EXPORT_SYMBOL(brcmu_dbg_hex_dump);
#endif				/* defined(DEBUG) */
Commit	Line	Data
5b435de0 AS	1	/*
	2	* Copyright (c) 2010 Broadcom Corporation
	3	*
	4	* Permission to use, copy, modify, and/or distribute this software for any
	5	* purpose with or without fee is hereby granted, provided that the above
	6	* copyright notice and this permission notice appear in all copies.
	7	*
	8	* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
	9	* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
	10	* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
	11	* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
	12	* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
	13	* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
	14	* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
	15	*/
	16
02f77195 JP	17	#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
02f77195 JP	18
5b435de0	19	#include <linux/netdevice.h>
b7a57e76	20	#include <linux/module.h>
20e5ca16	21
5b435de0 AS	22	#include <brcmu_utils.h>
	23
	24	MODULE_AUTHOR("Broadcom Corporation");
	25	MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
	26	MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
	27	MODULE_LICENSE("Dual BSD/GPL");
	28
	29	struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
	30	{
	31	struct sk_buff *skb;
	32
	33	skb = dev_alloc_skb(len);
	34	if (skb) {
	35	skb_put(skb, len);
	36	skb->priority = 0;
	37	}
	38
	39	return skb;
	40	}
	41	EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
	42
	43	/* Free the driver packet. Free the tag if present */
	44	void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
	45	{
1dacd198 DC	46	if (!skb)
1dacd198 DC	47	return;
130e380b	48
53ee4bc4	49	WARN_ON(skb->next);
130e380b	50	dev_kfree_skb_any(skb);
5b435de0 AS	51	}
	52	EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
	53
5b435de0 AS	54	/*
	55	* osl multiple-precedence packet queue
	56	* hi_prec is always >= the number of the highest non-empty precedence
	57	*/
	58	struct sk_buff brcmu_pktq_penq(struct pktq pq, int prec,
	59	struct sk_buff *p)
	60	{
ad3b8b39	61	struct sk_buff_head *q;
5b435de0 AS	62
	63	if (pktq_full(pq) \|\| pktq_pfull(pq, prec))
	64	return NULL;
	65
ad3b8b39 AS	66	q = &pq->q[prec].skblist;
ad3b8b39 AS	67	skb_queue_tail(q, p);
5b435de0 AS	68	pq->len++;
	69
	70	if (pq->hi_prec < prec)
	71	pq->hi_prec = (u8) prec;
	72
	73	return p;
	74	}
	75	EXPORT_SYMBOL(brcmu_pktq_penq);
	76
	77	struct sk_buff brcmu_pktq_penq_head(struct pktq pq, int prec,
	78	struct sk_buff *p)
	79	{
ad3b8b39	80	struct sk_buff_head *q;
5b435de0 AS	81
	82	if (pktq_full(pq) \|\| pktq_pfull(pq, prec))
	83	return NULL;
	84
ad3b8b39 AS	85	q = &pq->q[prec].skblist;
ad3b8b39 AS	86	skb_queue_head(q, p);
5b435de0 AS	87	pq->len++;
	88
	89	if (pq->hi_prec < prec)
	90	pq->hi_prec = (u8) prec;
	91
	92	return p;
	93	}
	94	EXPORT_SYMBOL(brcmu_pktq_penq_head);
	95
	96	struct sk_buff brcmu_pktq_pdeq(struct pktq pq, int prec)
	97	{
ad3b8b39	98	struct sk_buff_head *q;
5b435de0 AS	99	struct sk_buff *p;
5b435de0 AS	100
ad3b8b39 AS	101	q = &pq->q[prec].skblist;
ad3b8b39 AS	102	p = skb_dequeue(q);
5b435de0 AS	103	if (p == NULL)
	104	return NULL;
	105
5b435de0	106	pq->len--;
5b435de0 AS	107	return p;
	108	}
	109	EXPORT_SYMBOL(brcmu_pktq_pdeq);
	110
17f14d7c AS	111	/*
	112	* precedence based dequeue with match function. Passing a NULL pointer
	113	* for the match function parameter is considered to be a wildcard so
	114	* any packet on the queue is returned. In that case it is no different
	115	* from brcmu_pktq_pdeq() above.
	116	*/
	117	struct sk_buff brcmu_pktq_pdeq_match(struct pktq pq, int prec,
	118	bool (match_fn)(struct sk_buff skb,
	119	void arg), void arg)
	120	{
	121	struct sk_buff_head *q;
	122	struct sk_buff p, next;
	123
	124	q = &pq->q[prec].skblist;
	125	skb_queue_walk_safe(q, p, next) {
	126	if (match_fn == NULL \|\| match_fn(p, arg)) {
	127	skb_unlink(p, q);
	128	pq->len--;
	129	return p;
	130	}
	131	}
	132	return NULL;
	133	}
	134	EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
	135
5b435de0 AS	136	struct sk_buff brcmu_pktq_pdeq_tail(struct pktq pq, int prec)
5b435de0 AS	137	{
ad3b8b39 AS	138	struct sk_buff_head *q;
ad3b8b39 AS	139	struct sk_buff *p;
5b435de0	140
ad3b8b39 AS	141	q = &pq->q[prec].skblist;
ad3b8b39 AS	142	p = skb_dequeue_tail(q);
5b435de0 AS	143	if (p == NULL)
	144	return NULL;
	145
5b435de0	146	pq->len--;
5b435de0 AS	147	return p;
	148	}
	149	EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
	150
	151	void
	152	brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
	153	bool (fn)(struct sk_buff , void ), void arg)
	154	{
ad3b8b39 AS	155	struct sk_buff_head *q;
ad3b8b39 AS	156	struct sk_buff p, next;
5b435de0	157
ad3b8b39 AS	158	q = &pq->q[prec].skblist;
ad3b8b39 AS	159	skb_queue_walk_safe(q, p, next) {
5b435de0	160	if (fn == NULL \|\| (*fn) (p, arg)) {
ad3b8b39	161	skb_unlink(p, q);
5b435de0	162	brcmu_pkt_buf_free_skb(p);
5b435de0	163	pq->len--;
5b435de0 AS	164	}
5b435de0 AS	165	}
5b435de0 AS	166	}
	167	EXPORT_SYMBOL(brcmu_pktq_pflush);
	168
	169	void brcmu_pktq_flush(struct pktq *pq, bool dir,
	170	bool (fn)(struct sk_buff , void ), void arg)
	171	{
	172	int prec;
	173	for (prec = 0; prec < pq->num_prec; prec++)
	174	brcmu_pktq_pflush(pq, prec, dir, fn, arg);
	175	}
	176	EXPORT_SYMBOL(brcmu_pktq_flush);
	177
	178	void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
	179	{
	180	int prec;
	181
	182	/* pq is variable size; only zero out what's requested */
	183	memset(pq, 0,
	184	offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
	185
	186	pq->num_prec = (u16) num_prec;
	187
	188	pq->max = (u16) max_len;
	189
ad3b8b39	190	for (prec = 0; prec < num_prec; prec++) {
5b435de0	191	pq->q[prec].max = pq->max;
ad3b8b39 AS	192	skb_queue_head_init(&pq->q[prec].skblist);
ad3b8b39 AS	193	}
5b435de0 AS	194	}
	195	EXPORT_SYMBOL(brcmu_pktq_init);
	196
	197	struct sk_buff brcmu_pktq_peek_tail(struct pktq pq, int *prec_out)
	198	{
	199	int prec;
	200
	201	if (pq->len == 0)
	202	return NULL;
	203
	204	for (prec = 0; prec < pq->hi_prec; prec++)
ad3b8b39	205	if (!skb_queue_empty(&pq->q[prec].skblist))
5b435de0 AS	206	break;
	207
	208	if (prec_out)
	209	*prec_out = prec;
	210
ad3b8b39	211	return skb_peek_tail(&pq->q[prec].skblist);
5b435de0 AS	212	}
	213	EXPORT_SYMBOL(brcmu_pktq_peek_tail);
	214
	215	/* Return sum of lengths of a specific set of precedences */
	216	int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
	217	{
	218	int prec, len;
	219
	220	len = 0;
	221
	222	for (prec = 0; prec <= pq->hi_prec; prec++)
	223	if (prec_bmp & (1 << prec))
ad3b8b39	224	len += pq->q[prec].skblist.qlen;
5b435de0 AS	225
	226	return len;
	227	}
	228	EXPORT_SYMBOL(brcmu_pktq_mlen);
	229
	230	/* Priority dequeue from a specific set of precedences */
	231	struct sk_buff brcmu_pktq_mdeq(struct pktq pq, uint prec_bmp,
	232	int *prec_out)
	233	{
ad3b8b39	234	struct sk_buff_head *q;
5b435de0 AS	235	struct sk_buff *p;
	236	int prec;
	237
	238	if (pq->len == 0)
	239	return NULL;
	240
ad3b8b39 AS	241	while ((prec = pq->hi_prec) > 0 &&
ad3b8b39 AS	242	skb_queue_empty(&pq->q[prec].skblist))
5b435de0 AS	243	pq->hi_prec--;
5b435de0 AS	244
ad3b8b39 AS	245	while ((prec_bmp & (1 << prec)) == 0 \|\|
ad3b8b39 AS	246	skb_queue_empty(&pq->q[prec].skblist))
5b435de0 AS	247	if (prec-- == 0)
	248	return NULL;
	249
ad3b8b39 AS	250	q = &pq->q[prec].skblist;
ad3b8b39 AS	251	p = skb_dequeue(q);
5b435de0 AS	252	if (p == NULL)
	253	return NULL;
	254
ad3b8b39	255	pq->len--;
5b435de0 AS	256
	257	if (prec_out)
	258	*prec_out = prec;
	259
5b435de0 AS	260	return p;
	261	}
	262	EXPORT_SYMBOL(brcmu_pktq_mdeq);
	263
8ae74654	264	#if defined(DEBUG)
5b435de0 AS	265	/* pretty hex print a pkt buffer chain */
	266	void brcmu_prpkt(const char msg, struct sk_buff p0)
	267	{
	268	struct sk_buff *p;
	269
	270	if (msg && (msg[0] != '\0'))
18aad4f8	271	pr_debug("%s:\n", msg);
5b435de0 AS	272
	273	for (p = p0; p; p = p->next)
	274	print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
	275	}
	276	EXPORT_SYMBOL(brcmu_prpkt);
1e023829 JP	277
	278	void brcmu_dbg_hex_dump(const void data, size_t size, const char fmt, ...)
	279	{
	280	struct va_format vaf;
	281	va_list args;
	282
	283	va_start(args, fmt);
	284
	285	vaf.fmt = fmt;
	286	vaf.va = &args;
	287
	288	pr_debug("%pV", &vaf);
	289
	290	va_end(args);
	291
	292	print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size);
	293	}
	294	EXPORT_SYMBOL(brcmu_dbg_hex_dump);
8ae74654	295	#endif /* defined(DEBUG) */