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