[mirror_ubuntu-eoan-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/kernel.h>
#include <linux/random.h>
#include <linux/slab.h>
#include <linux/uwb.h>

#include "uwb-internal.h"


/*
 * Return the reason code for a reservations's DRP IE.
 */
static int uwb_rsv_reason_code(struct uwb_rsv *rsv)
{
	static const int reason_codes[] = {
		[UWB_RSV_STATE_O_INITIATED]          = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_O_PENDING]            = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_O_MODIFIED]           = UWB_DRP_REASON_MODIFIED,
		[UWB_RSV_STATE_O_ESTABLISHED]        = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_O_TO_BE_MOVED]        = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_O_MOVE_COMBINING]     = UWB_DRP_REASON_MODIFIED,
		[UWB_RSV_STATE_O_MOVE_REDUCING]      = UWB_DRP_REASON_MODIFIED,
		[UWB_RSV_STATE_O_MOVE_EXPANDING]     = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_ACCEPTED]           = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_CONFLICT]           = UWB_DRP_REASON_CONFLICT,
		[UWB_RSV_STATE_T_PENDING]            = UWB_DRP_REASON_PENDING,
		[UWB_RSV_STATE_T_DENIED]             = UWB_DRP_REASON_DENIED,
		[UWB_RSV_STATE_T_RESIZED]            = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
		[UWB_RSV_STATE_T_EXPANDING_PENDING]  = UWB_DRP_REASON_PENDING,
		[UWB_RSV_STATE_T_EXPANDING_DENIED]   = UWB_DRP_REASON_DENIED,
	};

	return reason_codes[rsv->state];
}

/*
 * Return the reason code for a reservations's companion DRP IE .
 */
static int uwb_rsv_companion_reason_code(struct uwb_rsv *rsv)
{
	static const int companion_reason_codes[] = {
		[UWB_RSV_STATE_O_MOVE_EXPANDING]     = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
		[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
		[UWB_RSV_STATE_T_EXPANDING_PENDING]  = UWB_DRP_REASON_PENDING,
		[UWB_RSV_STATE_T_EXPANDING_DENIED]   = UWB_DRP_REASON_DENIED,
	};

	return companion_reason_codes[rsv->state];
}

/*
 * Return the status bit for a reservations's DRP IE.
 */
int uwb_rsv_status(struct uwb_rsv *rsv)
{
	static const int statuses[] = {
		[UWB_RSV_STATE_O_INITIATED]          = 0,
		[UWB_RSV_STATE_O_PENDING]            = 0,
		[UWB_RSV_STATE_O_MODIFIED]           = 1,
		[UWB_RSV_STATE_O_ESTABLISHED]        = 1,
		[UWB_RSV_STATE_O_TO_BE_MOVED]        = 0,
		[UWB_RSV_STATE_O_MOVE_COMBINING]     = 1,
		[UWB_RSV_STATE_O_MOVE_REDUCING]      = 1,
		[UWB_RSV_STATE_O_MOVE_EXPANDING]     = 1,
		[UWB_RSV_STATE_T_ACCEPTED]           = 1,
		[UWB_RSV_STATE_T_CONFLICT]           = 0,
		[UWB_RSV_STATE_T_PENDING]            = 0,
		[UWB_RSV_STATE_T_DENIED]             = 0,
		[UWB_RSV_STATE_T_RESIZED]            = 1,
		[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
		[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 1,
		[UWB_RSV_STATE_T_EXPANDING_PENDING]  = 1,
		[UWB_RSV_STATE_T_EXPANDING_DENIED]   = 1,

	};

	return statuses[rsv->state];
}

/*
 * Return the status bit for a reservations's companion DRP IE .
 */
int uwb_rsv_companion_status(struct uwb_rsv *rsv)
{
	static const int companion_statuses[] = {
		[UWB_RSV_STATE_O_MOVE_EXPANDING]     = 0,
		[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
		[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 0,
		[UWB_RSV_STATE_T_EXPANDING_PENDING]  = 0,
		[UWB_RSV_STATE_T_EXPANDING_DENIED]   = 0,
	};

	return companion_statuses[rsv->state];
}

/*
 * 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;

	drp_ie = kzalloc(struct_size(drp_ie, allocs, UWB_NUM_ZONES),
			 GFP_KERNEL);
	if (drp_ie)
		drp_ie->hdr.element_id = UWB_IE_DRP;
	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 uwb_ie_drp *drp_ie;
	struct uwb_rsv_move *mv;
	int unsafe;

	if (rsv->state == UWB_RSV_STATE_NONE) {
		kfree(rsv->drp_ie);
		rsv->drp_ie = NULL;
		return 0;
	}

	unsafe = rsv->mas.unsafe ? 1 : 0;

	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_unsafe(drp_ie,       unsafe);
	uwb_ie_drp_set_tiebreaker(drp_ie,   rsv->tiebreaker);
	uwb_ie_drp_set_owner(drp_ie,        uwb_rsv_is_owner(rsv));
	uwb_ie_drp_set_status(drp_ie,       uwb_rsv_status(rsv));
	uwb_ie_drp_set_reason_code(drp_ie,  uwb_rsv_reason_code(rsv));
	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);

	if (uwb_rsv_has_two_drp_ies(rsv)) {
		mv = &rsv->mv;
		if (mv->companion_drp_ie == NULL) {
			mv->companion_drp_ie = uwb_drp_ie_alloc();
			if (mv->companion_drp_ie == NULL)
				return -ENOMEM;
		}
		drp_ie = mv->companion_drp_ie;

		/* keep all the same configuration of the main drp_ie */
		memcpy(drp_ie, rsv->drp_ie, sizeof(struct uwb_ie_drp));


		/* FIXME: handle properly the unsafe bit */
		uwb_ie_drp_set_unsafe(drp_ie,       1);
		uwb_ie_drp_set_status(drp_ie,       uwb_rsv_companion_status(rsv));
		uwb_ie_drp_set_reason_code(drp_ie,  uwb_rsv_companion_reason_code(rsv));

		uwb_drp_ie_from_bm(drp_ie, &mv->companion_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;

	bitmap_zero(bm->bm, UWB_NUM_MAS);

	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	*/
8cc13a09 DV	19	#include <linux/kernel.h>
8cc13a09 DV	20	#include <linux/random.h>
5a0e3ad6	21	#include <linux/slab.h>
8cc13a09 DV	22	#include <linux/uwb.h>
	23
	24	#include "uwb-internal.h"
	25
5b37717a SP	26
	27	/*
	28	* Return the reason code for a reservations's DRP IE.
	29	*/
67e51ec4	30	static int uwb_rsv_reason_code(struct uwb_rsv *rsv)
5b37717a SP	31	{
	32	static const int reason_codes[] = {
	33	[UWB_RSV_STATE_O_INITIATED] = UWB_DRP_REASON_ACCEPTED,
	34	[UWB_RSV_STATE_O_PENDING] = UWB_DRP_REASON_ACCEPTED,
	35	[UWB_RSV_STATE_O_MODIFIED] = UWB_DRP_REASON_MODIFIED,
	36	[UWB_RSV_STATE_O_ESTABLISHED] = UWB_DRP_REASON_ACCEPTED,
	37	[UWB_RSV_STATE_O_TO_BE_MOVED] = UWB_DRP_REASON_ACCEPTED,
	38	[UWB_RSV_STATE_O_MOVE_COMBINING] = UWB_DRP_REASON_MODIFIED,
	39	[UWB_RSV_STATE_O_MOVE_REDUCING] = UWB_DRP_REASON_MODIFIED,
	40	[UWB_RSV_STATE_O_MOVE_EXPANDING] = UWB_DRP_REASON_ACCEPTED,
	41	[UWB_RSV_STATE_T_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
	42	[UWB_RSV_STATE_T_CONFLICT] = UWB_DRP_REASON_CONFLICT,
	43	[UWB_RSV_STATE_T_PENDING] = UWB_DRP_REASON_PENDING,
	44	[UWB_RSV_STATE_T_DENIED] = UWB_DRP_REASON_DENIED,
	45	[UWB_RSV_STATE_T_RESIZED] = UWB_DRP_REASON_ACCEPTED,
	46	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
	47	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
	48	[UWB_RSV_STATE_T_EXPANDING_PENDING] = UWB_DRP_REASON_PENDING,
	49	[UWB_RSV_STATE_T_EXPANDING_DENIED] = UWB_DRP_REASON_DENIED,
	50	};
	51
	52	return reason_codes[rsv->state];
	53	}
	54
	55	/*
	56	* Return the reason code for a reservations's companion DRP IE .
	57	*/
67e51ec4	58	static int uwb_rsv_companion_reason_code(struct uwb_rsv *rsv)
5b37717a SP	59	{
	60	static const int companion_reason_codes[] = {
	61	[UWB_RSV_STATE_O_MOVE_EXPANDING] = UWB_DRP_REASON_ACCEPTED,
	62	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
	63	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
	64	[UWB_RSV_STATE_T_EXPANDING_PENDING] = UWB_DRP_REASON_PENDING,
	65	[UWB_RSV_STATE_T_EXPANDING_DENIED] = UWB_DRP_REASON_DENIED,
	66	};
	67
	68	return companion_reason_codes[rsv->state];
	69	}
	70
	71	/*
	72	* Return the status bit for a reservations's DRP IE.
	73	*/
	74	int uwb_rsv_status(struct uwb_rsv *rsv)
	75	{
	76	static const int statuses[] = {
	77	[UWB_RSV_STATE_O_INITIATED] = 0,
	78	[UWB_RSV_STATE_O_PENDING] = 0,
	79	[UWB_RSV_STATE_O_MODIFIED] = 1,
	80	[UWB_RSV_STATE_O_ESTABLISHED] = 1,
	81	[UWB_RSV_STATE_O_TO_BE_MOVED] = 0,
	82	[UWB_RSV_STATE_O_MOVE_COMBINING] = 1,
	83	[UWB_RSV_STATE_O_MOVE_REDUCING] = 1,
	84	[UWB_RSV_STATE_O_MOVE_EXPANDING] = 1,
	85	[UWB_RSV_STATE_T_ACCEPTED] = 1,
	86	[UWB_RSV_STATE_T_CONFLICT] = 0,
	87	[UWB_RSV_STATE_T_PENDING] = 0,
	88	[UWB_RSV_STATE_T_DENIED] = 0,
	89	[UWB_RSV_STATE_T_RESIZED] = 1,
	90	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
	91	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 1,
	92	[UWB_RSV_STATE_T_EXPANDING_PENDING] = 1,
	93	[UWB_RSV_STATE_T_EXPANDING_DENIED] = 1,
	94
	95	};
	96
	97	return statuses[rsv->state];
	98	}
	99
	100	/*
	101	* Return the status bit for a reservations's companion DRP IE .
	102	*/
	103	int uwb_rsv_companion_status(struct uwb_rsv *rsv)
	104	{
	105	static const int companion_statuses[] = {
	106	[UWB_RSV_STATE_O_MOVE_EXPANDING] = 0,
	107	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
	108	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 0,
	109	[UWB_RSV_STATE_T_EXPANDING_PENDING] = 0,
	110	[UWB_RSV_STATE_T_EXPANDING_DENIED] = 0,
	111	};
	112
	113	return companion_statuses[rsv->state];
	114	}
	115
8cc13a09 DV	116	/*
	117	* Allocate a DRP IE.
	118	*
	119	* To save having to free/allocate a DRP IE when its MAS changes,
	120	* enough memory is allocated for the maxiumum number of DRP
	121	* allocation fields. This gives an overhead per reservation of up to
	122	* (UWB_NUM_ZONES - 1) * 4 = 60 octets.
	123	*/
	124	static struct uwb_ie_drp *uwb_drp_ie_alloc(void)
	125	{
	126	struct uwb_ie_drp *drp_ie;
8cc13a09	127
6041828f GS	128	drp_ie = kzalloc(struct_size(drp_ie, allocs, UWB_NUM_ZONES),
6041828f GS	129	GFP_KERNEL);
8206948c	130	if (drp_ie)
8cc13a09	131	drp_ie->hdr.element_id = UWB_IE_DRP;
8cc13a09 DV	132	return drp_ie;
	133	}
	134
	135
	136	/*
	137	* Fill a DRP IE's allocation fields from a MAS bitmap.
	138	*/
	139	static void uwb_drp_ie_from_bm(struct uwb_ie_drp *drp_ie,
	140	struct uwb_mas_bm *mas)
	141	{
	142	int z, i, num_fields = 0, next = 0;
	143	struct uwb_drp_alloc *zones;
	144	__le16 current_bmp;
	145	DECLARE_BITMAP(tmp_bmp, UWB_NUM_MAS);
	146	DECLARE_BITMAP(tmp_mas_bm, UWB_MAS_PER_ZONE);
	147
	148	zones = drp_ie->allocs;
	149
	150	bitmap_copy(tmp_bmp, mas->bm, UWB_NUM_MAS);
	151
	152	/* Determine unique MAS bitmaps in zones from bitmap. */
	153	for (z = 0; z < UWB_NUM_ZONES; z++) {
	154	bitmap_copy(tmp_mas_bm, tmp_bmp, UWB_MAS_PER_ZONE);
	155	if (bitmap_weight(tmp_mas_bm, UWB_MAS_PER_ZONE) > 0) {
	156	bool found = false;
	157	current_bmp = (__le16) *tmp_mas_bm;
	158	for (i = 0; i < next; i++) {
	159	if (current_bmp == zones[i].mas_bm) {
	160	zones[i].zone_bm \|= 1 << z;
	161	found = true;
	162	break;
	163	}
	164	}
	165	if (!found) {
	166	num_fields++;
	167	zones[next].zone_bm = 1 << z;
	168	zones[next].mas_bm = current_bmp;
	169	next++;
	170	}
	171	}
	172	bitmap_shift_right(tmp_bmp, tmp_bmp, UWB_MAS_PER_ZONE, UWB_NUM_MAS);
	173	}
	174
	175	/* Store in format ready for transmission (le16). */
	176	for (i = 0; i < num_fields; i++) {
	177	drp_ie->allocs[i].zone_bm = cpu_to_le16(zones[i].zone_bm);
	178	drp_ie->allocs[i].mas_bm = cpu_to_le16(zones[i].mas_bm);
	179	}
	180
	181	drp_ie->hdr.length = sizeof(struct uwb_ie_drp) - sizeof(struct uwb_ie_hdr)
	182	+ num_fields * sizeof(struct uwb_drp_alloc);
	183	}
	184
	185	/**
	186	* uwb_drp_ie_update - update a reservation's DRP IE
	187	* @rsv: the reservation
	188	*/
	189	int uwb_drp_ie_update(struct uwb_rsv *rsv)
	190	{
8cc13a09	191	struct uwb_ie_drp *drp_ie;
5b37717a SP	192	struct uwb_rsv_move *mv;
5b37717a SP	193	int unsafe;
8cc13a09	194
5b37717a	195	if (rsv->state == UWB_RSV_STATE_NONE) {
8cc13a09 DV	196	kfree(rsv->drp_ie);
	197	rsv->drp_ie = NULL;
	198	return 0;
8cc13a09	199	}
d19000b8	200
5b37717a	201	unsafe = rsv->mas.unsafe ? 1 : 0;
8cc13a09 DV	202
	203	if (rsv->drp_ie == NULL) {
	204	rsv->drp_ie = uwb_drp_ie_alloc();
	205	if (rsv->drp_ie == NULL)
	206	return -ENOMEM;
	207	}
	208	drp_ie = rsv->drp_ie;
	209
5b37717a SP	210	uwb_ie_drp_set_unsafe(drp_ie, unsafe);
5b37717a SP	211	uwb_ie_drp_set_tiebreaker(drp_ie, rsv->tiebreaker);
8cc13a09	212	uwb_ie_drp_set_owner(drp_ie, uwb_rsv_is_owner(rsv));
5b37717a SP	213	uwb_ie_drp_set_status(drp_ie, uwb_rsv_status(rsv));
5b37717a SP	214	uwb_ie_drp_set_reason_code(drp_ie, uwb_rsv_reason_code(rsv));
8cc13a09 DV	215	uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);
	216	uwb_ie_drp_set_type(drp_ie, rsv->type);
	217
	218	if (uwb_rsv_is_owner(rsv)) {
	219	switch (rsv->target.type) {
	220	case UWB_RSV_TARGET_DEV:
	221	drp_ie->dev_addr = rsv->target.dev->dev_addr;
	222	break;
	223	case UWB_RSV_TARGET_DEVADDR:
	224	drp_ie->dev_addr = rsv->target.devaddr;
	225	break;
	226	}
	227	} else
	228	drp_ie->dev_addr = rsv->owner->dev_addr;
	229
	230	uwb_drp_ie_from_bm(drp_ie, &rsv->mas);
	231
5b37717a	232	if (uwb_rsv_has_two_drp_ies(rsv)) {
d19000b8	233	mv = &rsv->mv;
5b37717a SP	234	if (mv->companion_drp_ie == NULL) {
	235	mv->companion_drp_ie = uwb_drp_ie_alloc();
	236	if (mv->companion_drp_ie == NULL)
	237	return -ENOMEM;
	238	}
	239	drp_ie = mv->companion_drp_ie;
d19000b8	240
5b37717a SP	241	/* keep all the same configuration of the main drp_ie */
5b37717a SP	242	memcpy(drp_ie, rsv->drp_ie, sizeof(struct uwb_ie_drp));
d19000b8	243
5b37717a SP	244
	245	/* FIXME: handle properly the unsafe bit */
	246	uwb_ie_drp_set_unsafe(drp_ie, 1);
	247	uwb_ie_drp_set_status(drp_ie, uwb_rsv_companion_status(rsv));
	248	uwb_ie_drp_set_reason_code(drp_ie, uwb_rsv_companion_reason_code(rsv));
d19000b8	249
5b37717a SP	250	uwb_drp_ie_from_bm(drp_ie, &mv->companion_mas);
	251	}
	252
8cc13a09 DV	253	rsv->ie_valid = true;
	254	return 0;
	255	}
	256
	257	/*
	258	* Set MAS bits from given MAS bitmap in a single zone of large bitmap.
	259	*
	260	* We are given a zone id and the MAS bitmap of bits that need to be set in
	261	* this zone. Note that this zone may already have bits set and this only
	262	* adds settings - we cannot simply assign the MAS bitmap contents to the
	263	* zone contents. We iterate over the the bits (MAS) in the zone and set the
	264	* bits that are set in the given MAS bitmap.
	265	*/
	266	static
	267	void uwb_drp_ie_single_zone_to_bm(struct uwb_mas_bm *bm, u8 zone, u16 mas_bm)
	268	{
	269	int mas;
	270	u16 mas_mask;
	271
	272	for (mas = 0; mas < UWB_MAS_PER_ZONE; mas++) {
	273	mas_mask = 1 << mas;
	274	if (mas_bm & mas_mask)
	275	set_bit(zone * UWB_NUM_ZONES + mas, bm->bm);
	276	}
	277	}
	278
	279	/**
	280	* uwb_drp_ie_zones_to_bm - convert DRP allocation fields to a bitmap
	281	* @mas: MAS bitmap that will be populated to correspond to the
	282	* allocation fields in the DRP IE
	283	* @drp_ie: the DRP IE that contains the allocation fields.
	284	*
	285	* The input format is an array of MAS allocation fields (16 bit Zone
	286	* bitmap, 16 bit MAS bitmap) as described in [ECMA-368] section
	287	* 16.8.6. The output is a full 256 bit MAS bitmap.
	288	*
	289	* We go over all the allocation fields, for each allocation field we
	290	* know which zones are impacted. We iterate over all the zones
	291	* impacted and call a function that will set the correct MAS bits in
	292	* each zone.
	293	*/
	294	void uwb_drp_ie_to_bm(struct uwb_mas_bm bm, const struct uwb_ie_drp drp_ie)
	295	{
	296	int numallocs = (drp_ie->hdr.length - 4) / 4;
	297	const struct uwb_drp_alloc *alloc;
	298	int cnt;
	299	u16 zone_bm, mas_bm;
	300	u8 zone;
	301	u16 zone_mask;
	302
5b37717a SP	303	bitmap_zero(bm->bm, UWB_NUM_MAS);
5b37717a SP	304
8cc13a09 DV	305	for (cnt = 0; cnt < numallocs; cnt++) {
	306	alloc = &drp_ie->allocs[cnt];
	307	zone_bm = le16_to_cpu(alloc->zone_bm);
	308	mas_bm = le16_to_cpu(alloc->mas_bm);
	309	for (zone = 0; zone < UWB_NUM_ZONES; zone++) {
	310	zone_mask = 1 << zone;
	311	if (zone_bm & zone_mask)
	312	uwb_drp_ie_single_zone_to_bm(bm, zone, mas_bm);
	313	}
	314	}
	315	}
5b37717a	316