[mirror_ubuntu-artful-kernel.git] / drivers / uwb / drp-ie.c

/*
 * UWB DRP IE management.
 *
 * Copyright (C) 2005-2006 Intel Corporation
 * Copyright (C) 2008 Cambridge Silicon Radio Ltd.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License version
 * 2 as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <linux/version.h>
#include <linux/kernel.h>
#include <linux/random.h>
#include <linux/uwb.h>

#include "uwb-internal.h"

/*
 * Allocate a DRP IE.
 *
 * To save having to free/allocate a DRP IE when its MAS changes,
 * enough memory is allocated for the maxiumum number of DRP
 * allocation fields.  This gives an overhead per reservation of up to
 * (UWB_NUM_ZONES - 1) * 4 = 60 octets.
 */
static struct uwb_ie_drp *uwb_drp_ie_alloc(void)
{
	struct uwb_ie_drp *drp_ie;
	unsigned tiebreaker;

	drp_ie = kzalloc(sizeof(struct uwb_ie_drp) +
			UWB_NUM_ZONES * sizeof(struct uwb_drp_alloc),
			GFP_KERNEL);
	if (drp_ie) {
		drp_ie->hdr.element_id = UWB_IE_DRP;

		get_random_bytes(&tiebreaker, sizeof(unsigned));
		uwb_ie_drp_set_tiebreaker(drp_ie, tiebreaker & 1);
	}
	return drp_ie;
}


/*
 * Fill a DRP IE's allocation fields from a MAS bitmap.
 */
static void uwb_drp_ie_from_bm(struct uwb_ie_drp *drp_ie,
			       struct uwb_mas_bm *mas)
{
	int z, i, num_fields = 0, next = 0;
	struct uwb_drp_alloc *zones;
	__le16 current_bmp;
	DECLARE_BITMAP(tmp_bmp, UWB_NUM_MAS);
	DECLARE_BITMAP(tmp_mas_bm, UWB_MAS_PER_ZONE);

	zones = drp_ie->allocs;

	bitmap_copy(tmp_bmp, mas->bm, UWB_NUM_MAS);

	/* Determine unique MAS bitmaps in zones from bitmap. */
	for (z = 0; z < UWB_NUM_ZONES; z++) {
		bitmap_copy(tmp_mas_bm, tmp_bmp, UWB_MAS_PER_ZONE);
		if (bitmap_weight(tmp_mas_bm, UWB_MAS_PER_ZONE) > 0) {
			bool found = false;
			current_bmp = (__le16) *tmp_mas_bm;
			for (i = 0; i < next; i++) {
				if (current_bmp == zones[i].mas_bm) {
					zones[i].zone_bm |= 1 << z;
					found = true;
					break;
				}
			}
			if (!found)  {
				num_fields++;
				zones[next].zone_bm = 1 << z;
				zones[next].mas_bm = current_bmp;
				next++;
			}
		}
		bitmap_shift_right(tmp_bmp, tmp_bmp, UWB_MAS_PER_ZONE, UWB_NUM_MAS);
	}

	/* Store in format ready for transmission (le16). */
	for (i = 0; i < num_fields; i++) {
		drp_ie->allocs[i].zone_bm = cpu_to_le16(zones[i].zone_bm);
		drp_ie->allocs[i].mas_bm = cpu_to_le16(zones[i].mas_bm);
	}

	drp_ie->hdr.length = sizeof(struct uwb_ie_drp) - sizeof(struct uwb_ie_hdr)
		+ num_fields * sizeof(struct uwb_drp_alloc);
}

/**
 * uwb_drp_ie_update - update a reservation's DRP IE
 * @rsv: the reservation
 */
int uwb_drp_ie_update(struct uwb_rsv *rsv)
{
	struct device *dev = &rsv->rc->uwb_dev.dev;
	struct uwb_ie_drp *drp_ie;
	int reason_code, status;

	switch (rsv->state) {
	case UWB_RSV_STATE_NONE:
		kfree(rsv->drp_ie);
		rsv->drp_ie = NULL;
		return 0;
	case UWB_RSV_STATE_O_INITIATED:
		reason_code = UWB_DRP_REASON_ACCEPTED;
		status = 0;
		break;
	case UWB_RSV_STATE_O_PENDING:
		reason_code = UWB_DRP_REASON_ACCEPTED;
		status = 0;
		break;
	case UWB_RSV_STATE_O_MODIFIED:
		reason_code = UWB_DRP_REASON_MODIFIED;
		status = 1;
		break;
	case UWB_RSV_STATE_O_ESTABLISHED:
		reason_code = UWB_DRP_REASON_ACCEPTED;
		status = 1;
		break;
	case UWB_RSV_STATE_T_ACCEPTED:
		reason_code = UWB_DRP_REASON_ACCEPTED;
		status = 1;
		break;
	case UWB_RSV_STATE_T_DENIED:
		reason_code = UWB_DRP_REASON_DENIED;
		status = 0;
		break;
	default:
		dev_dbg(dev, "rsv with unhandled state (%d)\n", rsv->state);
		return -EINVAL;
	}

	if (rsv->drp_ie == NULL) {
		rsv->drp_ie = uwb_drp_ie_alloc();
		if (rsv->drp_ie == NULL)
			return -ENOMEM;
	}
	drp_ie = rsv->drp_ie;

	uwb_ie_drp_set_owner(drp_ie,        uwb_rsv_is_owner(rsv));
	uwb_ie_drp_set_status(drp_ie,       status);
	uwb_ie_drp_set_reason_code(drp_ie,  reason_code);
	uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);
	uwb_ie_drp_set_type(drp_ie,         rsv->type);

	if (uwb_rsv_is_owner(rsv)) {
		switch (rsv->target.type) {
		case UWB_RSV_TARGET_DEV:
			drp_ie->dev_addr = rsv->target.dev->dev_addr;
			break;
		case UWB_RSV_TARGET_DEVADDR:
			drp_ie->dev_addr = rsv->target.devaddr;
			break;
		}
	} else
		drp_ie->dev_addr = rsv->owner->dev_addr;

	uwb_drp_ie_from_bm(drp_ie, &rsv->mas);

	rsv->ie_valid = true;
	return 0;
}

/*
 * Set MAS bits from given MAS bitmap in a single zone of large bitmap.
 *
 * We are given a zone id and the MAS bitmap of bits that need to be set in
 * this zone. Note that this zone may already have bits set and this only
 * adds settings - we cannot simply assign the MAS bitmap contents to the
 * zone contents. We iterate over the the bits (MAS) in the zone and set the
 * bits that are set in the given MAS bitmap.
 */
static
void uwb_drp_ie_single_zone_to_bm(struct uwb_mas_bm *bm, u8 zone, u16 mas_bm)
{
	int mas;
	u16 mas_mask;

	for (mas = 0; mas < UWB_MAS_PER_ZONE; mas++) {
		mas_mask = 1 << mas;
		if (mas_bm & mas_mask)
			set_bit(zone * UWB_NUM_ZONES + mas, bm->bm);
	}
}

/**
 * uwb_drp_ie_zones_to_bm - convert DRP allocation fields to a bitmap
 * @mas:    MAS bitmap that will be populated to correspond to the
 *          allocation fields in the DRP IE
 * @drp_ie: the DRP IE that contains the allocation fields.
 *
 * The input format is an array of MAS allocation fields (16 bit Zone
 * bitmap, 16 bit MAS bitmap) as described in [ECMA-368] section
 * 16.8.6. The output is a full 256 bit MAS bitmap.
 *
 * We go over all the allocation fields, for each allocation field we
 * know which zones are impacted. We iterate over all the zones
 * impacted and call a function that will set the correct MAS bits in
 * each zone.
 */
void uwb_drp_ie_to_bm(struct uwb_mas_bm *bm, const struct uwb_ie_drp *drp_ie)
{
	int numallocs = (drp_ie->hdr.length - 4) / 4;
	const struct uwb_drp_alloc *alloc;
	int cnt;
	u16 zone_bm, mas_bm;
	u8 zone;
	u16 zone_mask;

	for (cnt = 0; cnt < numallocs; cnt++) {
		alloc = &drp_ie->allocs[cnt];
		zone_bm = le16_to_cpu(alloc->zone_bm);
		mas_bm = le16_to_cpu(alloc->mas_bm);
		for (zone = 0; zone < UWB_NUM_ZONES; zone++)   {
			zone_mask = 1 << zone;
			if (zone_bm & zone_mask)
				uwb_drp_ie_single_zone_to_bm(bm, zone, mas_bm);
		}
	}
}
Commit	Line	Data
8cc13a09 DV	1	/*
	2	* UWB DRP IE management.
	3	*
	4	* Copyright (C) 2005-2006 Intel Corporation
	5	* Copyright (C) 2008 Cambridge Silicon Radio Ltd.
	6	*
	7	* This program is free software; you can redistribute it and/or
	8	* modify it under the terms of the GNU General Public License version
	9	* 2 as published by the Free Software Foundation.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU General Public License
	17	* along with this program. If not, see <http://www.gnu.org/licenses/>.
	18	*/
	19	#include <linux/version.h>
	20	#include <linux/kernel.h>
	21	#include <linux/random.h>
	22	#include <linux/uwb.h>
	23
	24	#include "uwb-internal.h"
	25
	26	/*
	27	* Allocate a DRP IE.
	28	*
	29	* To save having to free/allocate a DRP IE when its MAS changes,
	30	* enough memory is allocated for the maxiumum number of DRP
	31	* allocation fields. This gives an overhead per reservation of up to
	32	* (UWB_NUM_ZONES - 1) * 4 = 60 octets.
	33	*/
	34	static struct uwb_ie_drp *uwb_drp_ie_alloc(void)
	35	{
	36	struct uwb_ie_drp *drp_ie;
	37	unsigned tiebreaker;
	38
	39	drp_ie = kzalloc(sizeof(struct uwb_ie_drp) +
	40	UWB_NUM_ZONES * sizeof(struct uwb_drp_alloc),
	41	GFP_KERNEL);
	42	if (drp_ie) {
	43	drp_ie->hdr.element_id = UWB_IE_DRP;
	44
	45	get_random_bytes(&tiebreaker, sizeof(unsigned));
	46	uwb_ie_drp_set_tiebreaker(drp_ie, tiebreaker & 1);
	47	}
	48	return drp_ie;
	49	}
	50
	51
	52	/*
	53	* Fill a DRP IE's allocation fields from a MAS bitmap.
	54	*/
	55	static void uwb_drp_ie_from_bm(struct uwb_ie_drp *drp_ie,
	56	struct uwb_mas_bm *mas)
	57	{
	58	int z, i, num_fields = 0, next = 0;
	59	struct uwb_drp_alloc *zones;
	60	__le16 current_bmp;
	61	DECLARE_BITMAP(tmp_bmp, UWB_NUM_MAS);
	62	DECLARE_BITMAP(tmp_mas_bm, UWB_MAS_PER_ZONE);
	63
	64	zones = drp_ie->allocs;
65
66	bitmap_copy(tmp_bmp, mas->bm, UWB_NUM_MAS);
67
68	/* Determine unique MAS bitmaps in zones from bitmap. */
69	for (z = 0; z < UWB_NUM_ZONES; z++) {
70	bitmap_copy(tmp_mas_bm, tmp_bmp, UWB_MAS_PER_ZONE);
71	if (bitmap_weight(tmp_mas_bm, UWB_MAS_PER_ZONE) > 0) {
72	bool found = false;
73	current_bmp = (__le16) *tmp_mas_bm;
74	for (i = 0; i < next; i++) {
75	if (current_bmp == zones[i].mas_bm) {
76	zones[i].zone_bm \|= 1 << z;
77	found = true;
78	break;
79	}
80	}
81	if (!found) {
82	num_fields++;
83	zones[next].zone_bm = 1 << z;
84	zones[next].mas_bm = current_bmp;
85	next++;
86	}
87	}
88	bitmap_shift_right(tmp_bmp, tmp_bmp, UWB_MAS_PER_ZONE, UWB_NUM_MAS);
89	}
90
91	/* Store in format ready for transmission (le16). */
92	for (i = 0; i < num_fields; i++) {
93	drp_ie->allocs[i].zone_bm = cpu_to_le16(zones[i].zone_bm);
94	drp_ie->allocs[i].mas_bm = cpu_to_le16(zones[i].mas_bm);
95	}
96
97	drp_ie->hdr.length = sizeof(struct uwb_ie_drp) - sizeof(struct uwb_ie_hdr)
98	+ num_fields * sizeof(struct uwb_drp_alloc);
99	}
100
101	/**
102	* uwb_drp_ie_update - update a reservation's DRP IE
103	* @rsv: the reservation
104	*/
105	int uwb_drp_ie_update(struct uwb_rsv *rsv)
106	{
107	struct device *dev = &rsv->rc->uwb_dev.dev;
108	struct uwb_ie_drp *drp_ie;
109	int reason_code, status;
110
111	switch (rsv->state) {
112	case UWB_RSV_STATE_NONE:
113	kfree(rsv->drp_ie);
114	rsv->drp_ie = NULL;
115	return 0;
116	case UWB_RSV_STATE_O_INITIATED:
117	reason_code = UWB_DRP_REASON_ACCEPTED;
118	status = 0;
119	break;
120	case UWB_RSV_STATE_O_PENDING:
121	reason_code = UWB_DRP_REASON_ACCEPTED;
122	status = 0;
123	break;
124	case UWB_RSV_STATE_O_MODIFIED:
125	reason_code = UWB_DRP_REASON_MODIFIED;
126	status = 1;
127	break;
128	case UWB_RSV_STATE_O_ESTABLISHED:
129	reason_code = UWB_DRP_REASON_ACCEPTED;
130	status = 1;
131	break;
132	case UWB_RSV_STATE_T_ACCEPTED:
133	reason_code = UWB_DRP_REASON_ACCEPTED;
134	status = 1;
135	break;
136	case UWB_RSV_STATE_T_DENIED:
137	reason_code = UWB_DRP_REASON_DENIED;
138	status = 0;
139	break;
140	default:
141	dev_dbg(dev, "rsv with unhandled state (%d)\n", rsv->state);
142	return -EINVAL;
143	}
144
145	if (rsv->drp_ie == NULL) {
146	rsv->drp_ie = uwb_drp_ie_alloc();
147	if (rsv->drp_ie == NULL)
148	return -ENOMEM;
149	}
150	drp_ie = rsv->drp_ie;
151
152	uwb_ie_drp_set_owner(drp_ie, uwb_rsv_is_owner(rsv));
153	uwb_ie_drp_set_status(drp_ie, status);
154	uwb_ie_drp_set_reason_code(drp_ie, reason_code);
155	uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);
156	uwb_ie_drp_set_type(drp_ie, rsv->type);
157
158	if (uwb_rsv_is_owner(rsv)) {
159	switch (rsv->target.type) {
160	case UWB_RSV_TARGET_DEV:
161	drp_ie->dev_addr = rsv->target.dev->dev_addr;
162	break;
163	case UWB_RSV_TARGET_DEVADDR:
164	drp_ie->dev_addr = rsv->target.devaddr;
165	break;
166	}
167	} else
168	drp_ie->dev_addr = rsv->owner->dev_addr;
169
170	uwb_drp_ie_from_bm(drp_ie, &rsv->mas);
171
172	rsv->ie_valid = true;
173	return 0;
174	}
175
176	/*
177	* Set MAS bits from given MAS bitmap in a single zone of large bitmap.
178	*
179	* We are given a zone id and the MAS bitmap of bits that need to be set in
180	* this zone. Note that this zone may already have bits set and this only
181	* adds settings - we cannot simply assign the MAS bitmap contents to the
182	* zone contents. We iterate over the the bits (MAS) in the zone and set the
183	* bits that are set in the given MAS bitmap.
184	*/
185	static
186	void uwb_drp_ie_single_zone_to_bm(struct uwb_mas_bm *bm, u8 zone, u16 mas_bm)
187	{
188	int mas;
189	u16 mas_mask;
190
191	for (mas = 0; mas < UWB_MAS_PER_ZONE; mas++) {
192	mas_mask = 1 << mas;
193	if (mas_bm & mas_mask)
194	set_bit(zone * UWB_NUM_ZONES + mas, bm->bm);
195	}
196	}
197
198	/**
199	* uwb_drp_ie_zones_to_bm - convert DRP allocation fields to a bitmap
200	* @mas: MAS bitmap that will be populated to correspond to the
201	* allocation fields in the DRP IE
202	* @drp_ie: the DRP IE that contains the allocation fields.
203	*
204	* The input format is an array of MAS allocation fields (16 bit Zone
205	* bitmap, 16 bit MAS bitmap) as described in [ECMA-368] section
206	* 16.8.6. The output is a full 256 bit MAS bitmap.
207	*
208	* We go over all the allocation fields, for each allocation field we
209	* know which zones are impacted. We iterate over all the zones
210	* impacted and call a function that will set the correct MAS bits in
211	* each zone.
212	*/
213	void uwb_drp_ie_to_bm(struct uwb_mas_bm bm, const struct uwb_ie_drp drp_ie)
214	{
215	int numallocs = (drp_ie->hdr.length - 4) / 4;
216	const struct uwb_drp_alloc *alloc;
217	int cnt;
218	u16 zone_bm, mas_bm;
219	u8 zone;
220	u16 zone_mask;
221
222	for (cnt = 0; cnt < numallocs; cnt++) {
223	alloc = &drp_ie->allocs[cnt];
224	zone_bm = le16_to_cpu(alloc->zone_bm);
225	mas_bm = le16_to_cpu(alloc->mas_bm);
226	for (zone = 0; zone < UWB_NUM_ZONES; zone++) {
227	zone_mask = 1 << zone;
228	if (zone_bm & zone_mask)
229	uwb_drp_ie_single_zone_to_bm(bm, zone, mas_bm);
230	}
231	}
232	}