diff options
author | David Vrabel <david.vrabel@csr.com> | 2008-09-17 11:34:09 -0400 |
---|---|---|
committer | David Vrabel <dv02@dv02pc01.europe.root.pri> | 2008-09-17 11:54:24 -0400 |
commit | 8cc13a09474bb30d15dbf449767bb6d0198a8bf8 (patch) | |
tree | 2423388e0a7e9bca0c6e6f7edd5f492887d38160 /drivers/uwb/drp-ie.c | |
parent | 22d203ecef9b0cc1fa8d8f64c935b451ca7d1022 (diff) |
uwb: add the UWB stack (reservation manager)
DRP and reservation management.
Signed-off-by: David Vrabel <david.vrabel@csr.com>
Diffstat (limited to 'drivers/uwb/drp-ie.c')
-rw-r--r-- | drivers/uwb/drp-ie.c | 232 |
1 files changed, 232 insertions, 0 deletions
diff --git a/drivers/uwb/drp-ie.c b/drivers/uwb/drp-ie.c new file mode 100644 index 000000000000..882724c5f126 --- /dev/null +++ b/drivers/uwb/drp-ie.c | |||
@@ -0,0 +1,232 @@ | |||
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 | } | ||