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 | } |