1 2 3#include <linux/slab.h> 4#include "uwb-internal.h" 5 6/** 7 * uwb_ie_next - get the next IE in a buffer 8 * @ptr: start of the buffer containing the IE data 9 * @len: length of the buffer 10 * 11 * Both @ptr and @len are updated so subsequent calls to uwb_ie_next() 12 * will get the next IE. 13 * 14 * NULL is returned (and @ptr and @len will not be updated) if there 15 * are no more IEs in the buffer or the buffer is too short. 16 */ 17struct uwb_ie_hdr *uwb_ie_next(void **ptr, size_t *len) 18{ 19 struct uwb_ie_hdr *hdr; 20 size_t ie_len; 21 22 if (*len < sizeof(struct uwb_ie_hdr)) 23 return NULL; 24 25 hdr = *ptr; 26 ie_len = sizeof(struct uwb_ie_hdr) + hdr->length; 27 28 if (*len < ie_len) 29 return NULL; 30 31 *ptr += ie_len; 32 *len -= ie_len; 33 34 return hdr; 35} 36EXPORT_SYMBOL_GPL(uwb_ie_next); 37 38/** 39 * uwb_ie_dump_hex - print IEs to a character buffer 40 * @ies: the IEs to print. 41 * @len: length of all the IEs. 42 * @buf: the destination buffer. 43 * @size: size of @buf. 44 * 45 * Returns the number of characters written. 46 */ 47int uwb_ie_dump_hex(const struct uwb_ie_hdr *ies, size_t len, 48 char *buf, size_t size) 49{ 50 void *ptr; 51 const struct uwb_ie_hdr *ie; 52 int r = 0; 53 u8 *d; 54 55 ptr = (void *)ies; 56 for (;;) { 57 ie = uwb_ie_next(&ptr, &len); 58 if (!ie) 59 break; 60 61 r += scnprintf(buf + r, size - r, "%02x %02x", 62 (unsigned)ie->element_id, 63 (unsigned)ie->length); 64 d = (uint8_t *)ie + sizeof(struct uwb_ie_hdr); 65 while (d != ptr && r < size) 66 r += scnprintf(buf + r, size - r, " %02x", (unsigned)*d++); 67 if (r < size) 68 buf[r++] = '\n'; 69 }; 70 71 return r; 72} 73 74/** 75 * Get the IEs that a radio controller is sending in its beacon 76 * 77 * @uwb_rc: UWB Radio Controller 78 * @returns: Size read from the system 79 * 80 * We don't need to lock the uwb_rc's mutex because we don't modify 81 * anything. Once done with the iedata buffer, call 82 * uwb_rc_ie_release(iedata). Don't call kfree on it. 83 */ 84static 85ssize_t uwb_rc_get_ie(struct uwb_rc *uwb_rc, struct uwb_rc_evt_get_ie **pget_ie) 86{ 87 ssize_t result; 88 struct device *dev = &uwb_rc->uwb_dev.dev; 89 struct uwb_rccb *cmd = NULL; 90 struct uwb_rceb *reply = NULL; 91 struct uwb_rc_evt_get_ie *get_ie; 92 93 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 94 if (cmd == NULL) 95 return -ENOMEM; 96 97 cmd->bCommandType = UWB_RC_CET_GENERAL; 98 cmd->wCommand = cpu_to_le16(UWB_RC_CMD_GET_IE); 99 result = uwb_rc_vcmd(uwb_rc, "GET_IE", cmd, sizeof(*cmd), 100 UWB_RC_CET_GENERAL, UWB_RC_CMD_GET_IE, 101 &reply); 102 kfree(cmd); 103 if (result < 0) 104 return result; 105 106 get_ie = container_of(reply, struct uwb_rc_evt_get_ie, rceb); 107 if (result < sizeof(*get_ie)) { 108 dev_err(dev, "not enough data returned for decoding GET IE " 109 "(%zu bytes received vs %zu needed)\n", 110 result, sizeof(*get_ie)); 111 return -EINVAL; 112 } else if (result < sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)) { 113 dev_err(dev, "not enough data returned for decoding GET IE " 114 "payload (%zu bytes received vs %zu needed)\n", result, 115 sizeof(*get_ie) + le16_to_cpu(get_ie->wIELength)); 116 return -EINVAL; 117 } 118 119 *pget_ie = get_ie; 120 return result; 121} 122 123 124/** 125 * Replace all IEs currently being transmitted by a device 126 * 127 * @cmd: pointer to the SET-IE command with the IEs to set 128 * @size: size of @buf 129 */ 130int uwb_rc_set_ie(struct uwb_rc *rc, struct uwb_rc_cmd_set_ie *cmd) 131{ 132 int result; 133 struct device *dev = &rc->uwb_dev.dev; 134 struct uwb_rc_evt_set_ie reply; 135 136 reply.rceb.bEventType = UWB_RC_CET_GENERAL; 137 reply.rceb.wEvent = UWB_RC_CMD_SET_IE; 138 result = uwb_rc_cmd(rc, "SET-IE", &cmd->rccb, 139 sizeof(*cmd) + le16_to_cpu(cmd->wIELength), 140 &reply.rceb, sizeof(reply)); 141 if (result < 0) 142 goto error_cmd; 143 else if (result != sizeof(reply)) { 144 dev_err(dev, "SET-IE: not enough data to decode reply " 145 "(%d bytes received vs %zu needed)\n", 146 result, sizeof(reply)); 147 result = -EIO; 148 } else if (reply.bResultCode != UWB_RC_RES_SUCCESS) { 149 dev_err(dev, "SET-IE: command execution failed: %s (%d)\n", 150 uwb_rc_strerror(reply.bResultCode), reply.bResultCode); 151 result = -EIO; 152 } else 153 result = 0; 154error_cmd: 155 return result; 156} 157 158/* Cleanup the whole IE management subsystem */ 159void uwb_rc_ie_init(struct uwb_rc *uwb_rc) 160{ 161 mutex_init(&uwb_rc->ies_mutex); 162} 163 164 165/** 166 * uwb_rc_ie_setup - setup a radio controller's IE manager 167 * @uwb_rc: the radio controller. 168 * 169 * The current set of IEs are obtained from the hardware with a GET-IE 170 * command (since the radio controller is not yet beaconing this will 171 * be just the hardware's MAC and PHY Capability IEs). 172 * 173 * Returns 0 on success; -ve on an error. 174 */ 175int uwb_rc_ie_setup(struct uwb_rc *uwb_rc) 176{ 177 struct uwb_rc_evt_get_ie *ie_info = NULL; 178 int capacity; 179 180 capacity = uwb_rc_get_ie(uwb_rc, &ie_info); 181 if (capacity < 0) 182 return capacity; 183 184 mutex_lock(&uwb_rc->ies_mutex); 185 186 uwb_rc->ies = (struct uwb_rc_cmd_set_ie *)ie_info; 187 uwb_rc->ies->rccb.bCommandType = UWB_RC_CET_GENERAL; 188 uwb_rc->ies->rccb.wCommand = cpu_to_le16(UWB_RC_CMD_SET_IE); 189 uwb_rc->ies_capacity = capacity; 190 191 mutex_unlock(&uwb_rc->ies_mutex); 192 193 return 0; 194} 195 196 197/* Cleanup the whole IE management subsystem */ 198void uwb_rc_ie_release(struct uwb_rc *uwb_rc) 199{ 200 kfree(uwb_rc->ies); 201 uwb_rc->ies = NULL; 202 uwb_rc->ies_capacity = 0; 203} 204 205 206static int uwb_rc_ie_add_one(struct uwb_rc *rc, const struct uwb_ie_hdr *new_ie) 207{ 208 struct uwb_rc_cmd_set_ie *new_ies; 209 void *ptr, *prev_ie; 210 struct uwb_ie_hdr *ie; 211 size_t length, new_ie_len, new_capacity, size, prev_size; 212 213 length = le16_to_cpu(rc->ies->wIELength); 214 new_ie_len = sizeof(struct uwb_ie_hdr) + new_ie->length; 215 new_capacity = sizeof(struct uwb_rc_cmd_set_ie) + length + new_ie_len; 216 217 if (new_capacity > rc->ies_capacity) { 218 new_ies = krealloc(rc->ies, new_capacity, GFP_KERNEL); 219 if (!new_ies) 220 return -ENOMEM; 221 rc->ies = new_ies; 222 } 223 224 ptr = rc->ies->IEData; 225 size = length; 226 for (;;) { 227 prev_ie = ptr; 228 prev_size = size; 229 ie = uwb_ie_next(&ptr, &size); 230 if (!ie || ie->element_id > new_ie->element_id) 231 break; 232 } 233 234 memmove(prev_ie + new_ie_len, prev_ie, prev_size); 235 memcpy(prev_ie, new_ie, new_ie_len); 236 rc->ies->wIELength = cpu_to_le16(length + new_ie_len); 237 238 return 0; 239} 240 241/** 242 * uwb_rc_ie_add - add new IEs to the radio controller's beacon 243 * @uwb_rc: the radio controller. 244 * @ies: the buffer containing the new IE or IEs to be added to 245 * the device's beacon. 246 * @size: length of all the IEs. 247 * 248 * According to WHCI 0.95 [4.13.6] the driver will only receive the RCEB 249 * after the device sent the first beacon that includes the IEs specified 250 * in the SET IE command. We thus cannot send this command if the device is 251 * not beaconing. Instead, a SET IE command will be sent later right after 252 * we start beaconing. 253 * 254 * Setting an IE on the device will overwrite all current IEs in device. So 255 * we take the current IEs being transmitted by the device, insert the 256 * new one, and call SET IE with all the IEs needed. 257 * 258 * Returns 0 on success; or -ENOMEM. 259 */ 260int uwb_rc_ie_add(struct uwb_rc *uwb_rc, 261 const struct uwb_ie_hdr *ies, size_t size) 262{ 263 int result = 0; 264 void *ptr; 265 const struct uwb_ie_hdr *ie; 266 267 mutex_lock(&uwb_rc->ies_mutex); 268 269 ptr = (void *)ies; 270 for (;;) { 271 ie = uwb_ie_next(&ptr, &size); 272 if (!ie) 273 break; 274 275 result = uwb_rc_ie_add_one(uwb_rc, ie); 276 if (result < 0) 277 break; 278 } 279 if (result >= 0) { 280 if (size == 0) { 281 if (uwb_rc->beaconing != -1) 282 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); 283 } else 284 result = -EINVAL; 285 } 286 287 mutex_unlock(&uwb_rc->ies_mutex); 288 289 return result; 290} 291EXPORT_SYMBOL_GPL(uwb_rc_ie_add); 292 293 294/* 295 * Remove an IE from internal cache 296 * 297 * We are dealing with our internal IE cache so no need to verify that the 298 * IEs are valid (it has been done already). 299 * 300 * Should be called with ies_mutex held 301 * 302 * We do not break out once an IE is found in the cache. It is currently 303 * possible to have more than one IE with the same ID included in the 304 * beacon. We don't reallocate, we just mark the size smaller. 305 */ 306static 307void uwb_rc_ie_cache_rm(struct uwb_rc *uwb_rc, enum uwb_ie to_remove) 308{ 309 struct uwb_ie_hdr *ie; 310 size_t len = le16_to_cpu(uwb_rc->ies->wIELength); 311 void *ptr; 312 size_t size; 313 314 ptr = uwb_rc->ies->IEData; 315 size = len; 316 for (;;) { 317 ie = uwb_ie_next(&ptr, &size); 318 if (!ie) 319 break; 320 if (ie->element_id == to_remove) { 321 len -= sizeof(struct uwb_ie_hdr) + ie->length; 322 memmove(ie, ptr, size); 323 ptr = ie; 324 } 325 } 326 uwb_rc->ies->wIELength = cpu_to_le16(len); 327} 328 329 330/** 331 * uwb_rc_ie_rm - remove an IE from the radio controller's beacon 332 * @uwb_rc: the radio controller. 333 * @element_id: the element ID of the IE to remove. 334 * 335 * Only IEs previously added with uwb_rc_ie_add() may be removed. 336 * 337 * Returns 0 on success; or -ve the SET-IE command to the radio 338 * controller failed. 339 */ 340int uwb_rc_ie_rm(struct uwb_rc *uwb_rc, enum uwb_ie element_id) 341{ 342 int result = 0; 343 344 mutex_lock(&uwb_rc->ies_mutex); 345 346 uwb_rc_ie_cache_rm(uwb_rc, element_id); 347 348 if (uwb_rc->beaconing != -1) 349 result = uwb_rc_set_ie(uwb_rc, uwb_rc->ies); 350 351 mutex_unlock(&uwb_rc->ies_mutex); 352 353 return result; 354} 355EXPORT_SYMBOL_GPL(uwb_rc_ie_rm); 356