uwb: add the UWB stack (reservation manager)

/*

 * Ultra Wide Band

 * DRP availability management

 *

 * Copyright (C) 2005-2006 Intel Corporation

 * Reinette Chatre <reinette.chatre@intel.com>

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

 *

 *

 * Manage DRP Availability (the MAS available for DRP

 * reservations). Thus:

 *

 * - Handle DRP Availability Change notifications

 *

 * - Allow the reservation manager to indicate MAS reserved/released

 *   by local (owned by/targeted at the radio controller)

 *   reservations.

 *

 * - Based on the two sources above, generate a DRP Availability IE to

 *   be included in the beacon.

 *

 * See also the documentation for struct uwb_drp_avail.

 */

#include <linux/errno.h>

#include <linux/module.h>

#include <linux/device.h>

#include <linux/bitmap.h>

#include "uwb-internal.h"

/**

 * uwb_drp_avail_init - initialize an RC's MAS availability

 *

 * All MAS are available initially.  The RC will inform use which

 * slots are used for the BP (it may change in size).

 */

void uwb_drp_avail_init(struct uwb_rc *rc)

{

	bitmap_fill(rc->drp_avail.global, UWB_NUM_MAS);

	bitmap_fill(rc->drp_avail.local, UWB_NUM_MAS);

	bitmap_fill(rc->drp_avail.pending, UWB_NUM_MAS);

}

/*

 * Determine MAS available for new local reservations.

 *

 * avail = global & local & pending

 */

static void uwb_drp_available(struct uwb_rc *rc, struct uwb_mas_bm *avail)

{

	bitmap_and(avail->bm, rc->drp_avail.global, rc->drp_avail.local, UWB_NUM_MAS);

	bitmap_and(avail->bm, avail->bm, rc->drp_avail.pending, UWB_NUM_MAS);

}

/**

 * uwb_drp_avail_reserve_pending - reserve MAS for a new reservation

 * @rc: the radio controller

 * @mas: the MAS to reserve

 *

 * Returns 0 on success, or -EBUSY if the MAS requested aren't available.

 */

int uwb_drp_avail_reserve_pending(struct uwb_rc *rc, struct uwb_mas_bm *mas)

{

	struct uwb_mas_bm avail;

	uwb_drp_available(rc, &avail);

	if (!bitmap_subset(mas->bm, avail.bm, UWB_NUM_MAS))

		return -EBUSY;

	bitmap_andnot(rc->drp_avail.pending, rc->drp_avail.pending, mas->bm, UWB_NUM_MAS);

	return 0;

}

/**

 * uwb_drp_avail_reserve - reserve MAS for an established reservation

 * @rc: the radio controller

 * @mas: the MAS to reserve

 */

void uwb_drp_avail_reserve(struct uwb_rc *rc, struct uwb_mas_bm *mas)

{

	bitmap_or(rc->drp_avail.pending, rc->drp_avail.pending, mas->bm, UWB_NUM_MAS);

	bitmap_andnot(rc->drp_avail.local, rc->drp_avail.local, mas->bm, UWB_NUM_MAS);

	rc->drp_avail.ie_valid = false;

}

/**

 * uwb_drp_avail_release - release MAS from a pending or established reservation

 * @rc: the radio controller

 * @mas: the MAS to release

 */

void uwb_drp_avail_release(struct uwb_rc *rc, struct uwb_mas_bm *mas)

{

	bitmap_or(rc->drp_avail.local, rc->drp_avail.local, mas->bm, UWB_NUM_MAS);

	bitmap_or(rc->drp_avail.pending, rc->drp_avail.pending, mas->bm, UWB_NUM_MAS);

	rc->drp_avail.ie_valid = false;

}

/**

 * uwb_drp_avail_ie_update - update the DRP Availability IE

 * @rc: the radio controller

 *

 * avail = global & local

 */

void uwb_drp_avail_ie_update(struct uwb_rc *rc)

{

	struct uwb_mas_bm avail;

	bitmap_and(avail.bm, rc->drp_avail.global, rc->drp_avail.local, UWB_NUM_MAS);

	rc->drp_avail.ie.hdr.element_id = UWB_IE_DRP_AVAILABILITY;

	rc->drp_avail.ie.hdr.length = UWB_NUM_MAS / 8;

	uwb_mas_bm_copy_le(rc->drp_avail.ie.bmp, &avail);

	rc->drp_avail.ie_valid = true;

}

/**

 * Create an unsigned long from a buffer containing a byte stream.

 *

 * @array: pointer to buffer

 * @itr:   index of buffer from where we start

 * @len:   the buffer's remaining size may not be exact multiple of

 *         sizeof(unsigned long), @len is the length of buffer that needs

 *         to be converted. This will be sizeof(unsigned long) or smaller

 *         (BUG if not). If it is smaller then we will pad the remaining

 *         space of the result with zeroes.

 */

static

unsigned long get_val(u8 *array, size_t itr, size_t len)

{

	unsigned long val = 0;

	size_t top = itr + len;

	BUG_ON(len > sizeof(val));

	while (itr < top) {

		val <<= 8;

		val |= array[top - 1];

		top--;

	}

	val <<= 8 * (sizeof(val) - len); /* padding */

	return val;

}

/**

 * Initialize bitmap from data buffer.

 *

 * The bitmap to be converted could come from a IE, for example a

 * DRP Availability IE.

 * From ECMA-368 1.0 [16.8.7]: "

 * octets: 1            1               N * (0 to 32)

 *         Element ID   Length (=N)     DRP Availability Bitmap

 *

 * The DRP Availability Bitmap field is up to 256 bits long, one

 * bit for each MAS in the superframe, where the least-significant

 * bit of the field corresponds to the first MAS in the superframe

 * and successive bits correspond to successive MASs."

 *

 * The DRP Availability bitmap is in octets from 0 to 32, so octet

 * 32 contains bits for MAS 1-8, etc. If the bitmap is smaller than 32

 * octets, the bits in octets not included at the end of the bitmap are

 * treated as zero. In this case (when the bitmap is smaller than 32

 * octets) the MAS represented range from MAS 1 to MAS (size of bitmap)

 * with the last octet still containing bits for MAS 1-8, etc.

 *

 * For example:

 * F00F0102 03040506 0708090A 0B0C0D0E 0F010203

 * ^^^^

 * ||||

 * ||||

 * |||\LSB of byte is MAS 9

 * ||\MSB of byte is MAS 16

 * |\LSB of first byte is MAS 1

 * \ MSB of byte is MAS 8

 *

 * An example of this encoding can be found in ECMA-368 Annex-D [Table D.11]

 *

 * The resulting bitmap will have the following mapping:

 *	bit position 0 == MAS 1

 *	bit position 1 == MAS 2

 *	...

 *	bit position (UWB_NUM_MAS - 1) == MAS UWB_NUM_MAS

 *

 * @bmp_itr:	pointer to bitmap (can be declared with DECLARE_BITMAP)

 * @buffer:	pointer to buffer containing bitmap data in big endian

 *              format (MSB first)

 * @buffer_size:number of bytes with which bitmap should be initialized

 */

static

void buffer_to_bmp(unsigned long *bmp_itr, void *_buffer,

		   size_t buffer_size)

{

	u8 *buffer = _buffer;

	size_t itr, len;

	unsigned long val;

	itr = 0;

	while (itr < buffer_size) {

		len = buffer_size - itr >= sizeof(val) ?

			sizeof(val) : buffer_size - itr;

		val = get_val(buffer, itr, len);

		bmp_itr[itr / sizeof(val)] = val;

		itr += sizeof(val);

	}

}

/**

 * Extract DRP Availability bitmap from the notification.

 *

 * The notification that comes in contains a bitmap of (UWB_NUM_MAS / 8) bytes

 * We convert that to our internal representation.

 */

static

int uwbd_evt_get_drp_avail(struct uwb_event *evt, unsigned long *bmp)

{

	struct device *dev = &evt->rc->uwb_dev.dev;

	struct uwb_rc_evt_drp_avail *drp_evt;

	int result = -EINVAL;

	/* Is there enough data to decode the event? */

	if (evt->notif.size < sizeof(*drp_evt)) {

		dev_err(dev, "DRP Availability Change: Not enough "

			"data to decode event [%zu bytes, %zu "

			"needed]\n", evt->notif.size, sizeof(*drp_evt));

		goto error;

	}

	drp_evt = container_of(evt->notif.rceb, struct uwb_rc_evt_drp_avail, rceb);

	buffer_to_bmp(bmp, drp_evt->bmp, UWB_NUM_MAS/8);

	result = 0;

error:

	return result;

}

/**

 * Process an incoming DRP Availability notification.

 *

 * @evt:	Event information (packs the actual event data, which

 *              radio controller it came to, etc).

 *

 * @returns:    0 on success (so uwbd() frees the event buffer), < 0

 *              on error.

 *

 * According to ECMA-368 1.0 [16.8.7], bits set to ONE indicate that

 * the MAS slot is available, bits set to ZERO indicate that the slot

 * is busy.

 *

 * So we clear available slots, we set used slots :)

 *

 * The notification only marks non-availability based on the BP and

 * received DRP IEs that are not for this radio controller.  A copy of

 * this bitmap is needed to generate the real availability (which

 * includes local and pending reservations).

 *

 * The DRP Availability IE that this radio controller emits will need

 * to be updated.

 */

int uwbd_evt_handle_rc_drp_avail(struct uwb_event *evt)

{

	int result;

	struct uwb_rc *rc = evt->rc;

	DECLARE_BITMAP(bmp, UWB_NUM_MAS);

	result = uwbd_evt_get_drp_avail(evt, bmp);

	if (result < 0)

		return result;

	mutex_lock(&rc->rsvs_mutex);

	bitmap_copy(rc->drp_avail.global, bmp, UWB_NUM_MAS);

	rc->drp_avail.ie_valid = false;

	mutex_unlock(&rc->rsvs_mutex);

	uwb_rsv_sched_update(rc);

	return 0;

}

/*

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

		}

	}

}