ie.c 9.6 KB

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