drivers/uwb/drp-ie.c

   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 }