Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2011 Intel Corporation. All rights reserved. |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify |
| 5 | * it under the terms of the GNU General Public License as published by |
| 6 | * the Free Software Foundation; either version 2 of the License, or |
| 7 | * (at your option) any later version. |
| 8 | * |
| 9 | * This program is distributed in the hope that it will be useful, |
| 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 12 | * GNU General Public License for more details. |
| 13 | * |
| 14 | * You should have received a copy of the GNU General Public License |
| 15 | * along with this program; if not, write to the |
| 16 | * Free Software Foundation, Inc., |
| 17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
| 18 | */ |
| 19 | |
| 20 | #define pr_fmt(fmt) "llcp: %s: " fmt, __func__ |
| 21 | |
| 22 | #include <linux/init.h> |
| 23 | #include <linux/kernel.h> |
| 24 | #include <linux/list.h> |
| 25 | #include <linux/nfc.h> |
| 26 | |
| 27 | #include "../nfc.h" |
| 28 | #include "llcp.h" |
| 29 | |
| 30 | static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; |
| 31 | |
| 32 | static struct list_head llcp_devices; |
| 33 | |
| 34 | static void nfc_llcp_socket_release(struct nfc_llcp_local *local) |
| 35 | { |
| 36 | struct nfc_llcp_sock *parent, *s, *n; |
| 37 | struct sock *sk, *parent_sk; |
| 38 | int i; |
| 39 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 40 | mutex_lock(&local->socket_lock); |
| 41 | |
| 42 | for (i = 0; i < LLCP_MAX_SAP; i++) { |
| 43 | parent = local->sockets[i]; |
| 44 | if (parent == NULL) |
| 45 | continue; |
| 46 | |
| 47 | /* Release all child sockets */ |
| 48 | list_for_each_entry_safe(s, n, &parent->list, list) { |
Samuel Ortiz | 40c75f8 | 2012-03-05 01:03:51 +0100 | [diff] [blame] | 49 | list_del_init(&s->list); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 50 | sk = &s->sk; |
| 51 | |
| 52 | lock_sock(sk); |
| 53 | |
| 54 | if (sk->sk_state == LLCP_CONNECTED) |
| 55 | nfc_put_device(s->dev); |
| 56 | |
| 57 | sk->sk_state = LLCP_CLOSED; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 58 | |
| 59 | release_sock(sk); |
Samuel Ortiz | 40c75f8 | 2012-03-05 01:03:51 +0100 | [diff] [blame] | 60 | |
| 61 | sock_orphan(sk); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 62 | } |
| 63 | |
| 64 | parent_sk = &parent->sk; |
| 65 | |
| 66 | lock_sock(parent_sk); |
| 67 | |
| 68 | if (parent_sk->sk_state == LLCP_LISTEN) { |
| 69 | struct nfc_llcp_sock *lsk, *n; |
| 70 | struct sock *accept_sk; |
| 71 | |
| 72 | list_for_each_entry_safe(lsk, n, &parent->accept_queue, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 73 | accept_queue) { |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 74 | accept_sk = &lsk->sk; |
| 75 | lock_sock(accept_sk); |
| 76 | |
| 77 | nfc_llcp_accept_unlink(accept_sk); |
| 78 | |
| 79 | accept_sk->sk_state = LLCP_CLOSED; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 80 | |
| 81 | release_sock(accept_sk); |
| 82 | |
| 83 | sock_orphan(accept_sk); |
| 84 | } |
| 85 | } |
| 86 | |
| 87 | if (parent_sk->sk_state == LLCP_CONNECTED) |
| 88 | nfc_put_device(parent->dev); |
| 89 | |
| 90 | parent_sk->sk_state = LLCP_CLOSED; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 91 | |
| 92 | release_sock(parent_sk); |
Samuel Ortiz | 40c75f8 | 2012-03-05 01:03:51 +0100 | [diff] [blame] | 93 | |
| 94 | sock_orphan(parent_sk); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 95 | } |
| 96 | |
| 97 | mutex_unlock(&local->socket_lock); |
| 98 | } |
| 99 | |
Samuel Ortiz | c7aa122 | 2012-05-04 11:24:16 +0200 | [diff] [blame^] | 100 | struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) |
| 101 | { |
| 102 | kref_get(&local->ref); |
| 103 | |
| 104 | return local; |
| 105 | } |
| 106 | |
| 107 | static void local_release(struct kref *ref) |
| 108 | { |
| 109 | struct nfc_llcp_local *local; |
| 110 | |
| 111 | local = container_of(ref, struct nfc_llcp_local, ref); |
| 112 | |
| 113 | list_del(&local->list); |
| 114 | nfc_llcp_socket_release(local); |
| 115 | del_timer_sync(&local->link_timer); |
| 116 | skb_queue_purge(&local->tx_queue); |
| 117 | destroy_workqueue(local->tx_wq); |
| 118 | destroy_workqueue(local->rx_wq); |
| 119 | kfree_skb(local->rx_pending); |
| 120 | kfree(local); |
| 121 | } |
| 122 | |
| 123 | int nfc_llcp_local_put(struct nfc_llcp_local *local) |
| 124 | { |
| 125 | return kref_put(&local->ref, local_release); |
| 126 | } |
| 127 | |
Samuel Ortiz | b9a76f1 | 2012-03-05 01:03:40 +0100 | [diff] [blame] | 128 | static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local) |
| 129 | { |
| 130 | mutex_lock(&local->sdp_lock); |
| 131 | |
| 132 | local->local_wks = 0; |
| 133 | local->local_sdp = 0; |
| 134 | local->local_sap = 0; |
| 135 | |
| 136 | mutex_unlock(&local->sdp_lock); |
| 137 | } |
| 138 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 139 | static void nfc_llcp_timeout_work(struct work_struct *work) |
| 140 | { |
| 141 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 142 | timeout_work); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 143 | |
| 144 | nfc_dep_link_down(local->dev); |
| 145 | } |
| 146 | |
| 147 | static void nfc_llcp_symm_timer(unsigned long data) |
| 148 | { |
| 149 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; |
| 150 | |
| 151 | pr_err("SYMM timeout\n"); |
| 152 | |
| 153 | queue_work(local->timeout_wq, &local->timeout_work); |
| 154 | } |
| 155 | |
| 156 | struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) |
| 157 | { |
| 158 | struct nfc_llcp_local *local, *n; |
| 159 | |
| 160 | list_for_each_entry_safe(local, n, &llcp_devices, list) |
| 161 | if (local->dev == dev) |
| 162 | return local; |
| 163 | |
| 164 | pr_debug("No device found\n"); |
| 165 | |
| 166 | return NULL; |
| 167 | } |
| 168 | |
| 169 | static char *wks[] = { |
| 170 | NULL, |
| 171 | NULL, /* SDP */ |
| 172 | "urn:nfc:sn:ip", |
| 173 | "urn:nfc:sn:obex", |
| 174 | "urn:nfc:sn:snep", |
| 175 | }; |
| 176 | |
| 177 | static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) |
| 178 | { |
| 179 | int sap, num_wks; |
| 180 | |
| 181 | pr_debug("%s\n", service_name); |
| 182 | |
| 183 | if (service_name == NULL) |
| 184 | return -EINVAL; |
| 185 | |
| 186 | num_wks = ARRAY_SIZE(wks); |
| 187 | |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 188 | for (sap = 0; sap < num_wks; sap++) { |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 189 | if (wks[sap] == NULL) |
| 190 | continue; |
| 191 | |
| 192 | if (strncmp(wks[sap], service_name, service_name_len) == 0) |
| 193 | return sap; |
| 194 | } |
| 195 | |
| 196 | return -EINVAL; |
| 197 | } |
| 198 | |
| 199 | u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 200 | struct nfc_llcp_sock *sock) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 201 | { |
| 202 | mutex_lock(&local->sdp_lock); |
| 203 | |
| 204 | if (sock->service_name != NULL && sock->service_name_len > 0) { |
| 205 | int ssap = nfc_llcp_wks_sap(sock->service_name, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 206 | sock->service_name_len); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 207 | |
| 208 | if (ssap > 0) { |
| 209 | pr_debug("WKS %d\n", ssap); |
| 210 | |
| 211 | /* This is a WKS, let's check if it's free */ |
| 212 | if (local->local_wks & BIT(ssap)) { |
| 213 | mutex_unlock(&local->sdp_lock); |
| 214 | |
| 215 | return LLCP_SAP_MAX; |
| 216 | } |
| 217 | |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 218 | set_bit(ssap, &local->local_wks); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 219 | mutex_unlock(&local->sdp_lock); |
| 220 | |
| 221 | return ssap; |
| 222 | } |
| 223 | |
| 224 | /* |
| 225 | * This is not a well known service, |
| 226 | * we should try to find a local SDP free spot |
| 227 | */ |
| 228 | ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); |
| 229 | if (ssap == LLCP_SDP_NUM_SAP) { |
| 230 | mutex_unlock(&local->sdp_lock); |
| 231 | |
| 232 | return LLCP_SAP_MAX; |
| 233 | } |
| 234 | |
| 235 | pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); |
| 236 | |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 237 | set_bit(ssap, &local->local_sdp); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 238 | mutex_unlock(&local->sdp_lock); |
| 239 | |
| 240 | return LLCP_WKS_NUM_SAP + ssap; |
| 241 | |
| 242 | } else if (sock->ssap != 0) { |
| 243 | if (sock->ssap < LLCP_WKS_NUM_SAP) { |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 244 | if (!test_bit(sock->ssap, &local->local_wks)) { |
| 245 | set_bit(sock->ssap, &local->local_wks); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 246 | mutex_unlock(&local->sdp_lock); |
| 247 | |
| 248 | return sock->ssap; |
| 249 | } |
| 250 | |
| 251 | } else if (sock->ssap < LLCP_SDP_NUM_SAP) { |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 252 | if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP, |
| 253 | &local->local_sdp)) { |
| 254 | set_bit(sock->ssap - LLCP_WKS_NUM_SAP, |
| 255 | &local->local_sdp); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 256 | mutex_unlock(&local->sdp_lock); |
| 257 | |
| 258 | return sock->ssap; |
| 259 | } |
| 260 | } |
| 261 | } |
| 262 | |
| 263 | mutex_unlock(&local->sdp_lock); |
| 264 | |
| 265 | return LLCP_SAP_MAX; |
| 266 | } |
| 267 | |
| 268 | u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) |
| 269 | { |
| 270 | u8 local_ssap; |
| 271 | |
| 272 | mutex_lock(&local->sdp_lock); |
| 273 | |
| 274 | local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); |
| 275 | if (local_ssap == LLCP_LOCAL_NUM_SAP) { |
| 276 | mutex_unlock(&local->sdp_lock); |
| 277 | return LLCP_SAP_MAX; |
| 278 | } |
| 279 | |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 280 | set_bit(local_ssap, &local->local_sap); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 281 | |
| 282 | mutex_unlock(&local->sdp_lock); |
| 283 | |
| 284 | return local_ssap + LLCP_LOCAL_SAP_OFFSET; |
| 285 | } |
| 286 | |
| 287 | void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) |
| 288 | { |
| 289 | u8 local_ssap; |
| 290 | unsigned long *sdp; |
| 291 | |
| 292 | if (ssap < LLCP_WKS_NUM_SAP) { |
| 293 | local_ssap = ssap; |
| 294 | sdp = &local->local_wks; |
| 295 | } else if (ssap < LLCP_LOCAL_NUM_SAP) { |
| 296 | local_ssap = ssap - LLCP_WKS_NUM_SAP; |
| 297 | sdp = &local->local_sdp; |
| 298 | } else if (ssap < LLCP_MAX_SAP) { |
| 299 | local_ssap = ssap - LLCP_LOCAL_NUM_SAP; |
| 300 | sdp = &local->local_sap; |
| 301 | } else { |
| 302 | return; |
| 303 | } |
| 304 | |
| 305 | mutex_lock(&local->sdp_lock); |
| 306 | |
Samuel Ortiz | 1762c17 | 2012-03-05 01:03:38 +0100 | [diff] [blame] | 307 | clear_bit(local_ssap, sdp); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 308 | |
| 309 | mutex_unlock(&local->sdp_lock); |
| 310 | } |
| 311 | |
Samuel Ortiz | 47807d3 | 2012-03-05 01:03:50 +0100 | [diff] [blame] | 312 | u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 313 | { |
| 314 | struct nfc_llcp_local *local; |
| 315 | |
| 316 | local = nfc_llcp_find_local(dev); |
| 317 | if (local == NULL) { |
| 318 | *general_bytes_len = 0; |
| 319 | return NULL; |
| 320 | } |
| 321 | |
| 322 | *general_bytes_len = local->gb_len; |
| 323 | |
| 324 | return local->gb; |
| 325 | } |
| 326 | |
| 327 | static int nfc_llcp_build_gb(struct nfc_llcp_local *local) |
| 328 | { |
| 329 | u8 *gb_cur, *version_tlv, version, version_length; |
| 330 | u8 *lto_tlv, lto, lto_length; |
| 331 | u8 *wks_tlv, wks_length; |
Samuel Ortiz | 56d5876 | 2012-04-10 19:43:19 +0200 | [diff] [blame] | 332 | u8 *miux_tlv, miux_length; |
| 333 | __be16 miux; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 334 | u8 gb_len = 0; |
| 335 | |
| 336 | version = LLCP_VERSION_11; |
| 337 | version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 338 | 1, &version_length); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 339 | gb_len += version_length; |
| 340 | |
| 341 | /* 1500 ms */ |
| 342 | lto = 150; |
Samuel Ortiz | 91b0ade | 2012-04-10 19:43:20 +0200 | [diff] [blame] | 343 | lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, <o, 1, <o_length); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 344 | gb_len += lto_length; |
| 345 | |
| 346 | pr_debug("Local wks 0x%lx\n", local->local_wks); |
| 347 | wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 348 | &wks_length); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 349 | gb_len += wks_length; |
| 350 | |
Samuel Ortiz | 56d5876 | 2012-04-10 19:43:19 +0200 | [diff] [blame] | 351 | miux = cpu_to_be16(LLCP_MAX_MIUX); |
| 352 | miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, |
| 353 | &miux_length); |
| 354 | gb_len += miux_length; |
| 355 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 356 | gb_len += ARRAY_SIZE(llcp_magic); |
| 357 | |
| 358 | if (gb_len > NFC_MAX_GT_LEN) { |
| 359 | kfree(version_tlv); |
| 360 | return -EINVAL; |
| 361 | } |
| 362 | |
| 363 | gb_cur = local->gb; |
| 364 | |
| 365 | memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); |
| 366 | gb_cur += ARRAY_SIZE(llcp_magic); |
| 367 | |
| 368 | memcpy(gb_cur, version_tlv, version_length); |
| 369 | gb_cur += version_length; |
| 370 | |
| 371 | memcpy(gb_cur, lto_tlv, lto_length); |
| 372 | gb_cur += lto_length; |
| 373 | |
| 374 | memcpy(gb_cur, wks_tlv, wks_length); |
| 375 | gb_cur += wks_length; |
| 376 | |
Samuel Ortiz | 56d5876 | 2012-04-10 19:43:19 +0200 | [diff] [blame] | 377 | memcpy(gb_cur, miux_tlv, miux_length); |
| 378 | gb_cur += miux_length; |
| 379 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 380 | kfree(version_tlv); |
| 381 | kfree(lto_tlv); |
| 382 | |
| 383 | local->gb_len = gb_len; |
| 384 | |
| 385 | return 0; |
| 386 | } |
| 387 | |
| 388 | int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) |
| 389 | { |
| 390 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); |
| 391 | |
| 392 | if (local == NULL) { |
| 393 | pr_err("No LLCP device\n"); |
| 394 | return -ENODEV; |
| 395 | } |
| 396 | |
| 397 | memset(local->remote_gb, 0, NFC_MAX_GT_LEN); |
| 398 | memcpy(local->remote_gb, gb, gb_len); |
| 399 | local->remote_gb_len = gb_len; |
| 400 | |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 401 | if (local->remote_gb == NULL || local->remote_gb_len == 0) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 402 | return -ENODEV; |
| 403 | |
| 404 | if (memcmp(local->remote_gb, llcp_magic, 3)) { |
| 405 | pr_err("MAC does not support LLCP\n"); |
| 406 | return -EINVAL; |
| 407 | } |
| 408 | |
| 409 | return nfc_llcp_parse_tlv(local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 410 | &local->remote_gb[3], |
| 411 | local->remote_gb_len - 3); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 412 | } |
| 413 | |
| 414 | static void nfc_llcp_tx_work(struct work_struct *work) |
| 415 | { |
| 416 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 417 | tx_work); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 418 | struct sk_buff *skb; |
| 419 | |
| 420 | skb = skb_dequeue(&local->tx_queue); |
| 421 | if (skb != NULL) { |
| 422 | pr_debug("Sending pending skb\n"); |
Samuel Ortiz | 4be646e | 2012-04-10 19:43:13 +0200 | [diff] [blame] | 423 | print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET, |
| 424 | 16, 1, skb->data, skb->len, true); |
| 425 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 426 | nfc_data_exchange(local->dev, local->target_idx, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 427 | skb, nfc_llcp_recv, local); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 428 | } else { |
| 429 | nfc_llcp_send_symm(local->dev); |
| 430 | } |
| 431 | |
| 432 | mod_timer(&local->link_timer, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 433 | jiffies + msecs_to_jiffies(local->remote_lto)); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 434 | } |
| 435 | |
| 436 | static u8 nfc_llcp_dsap(struct sk_buff *pdu) |
| 437 | { |
| 438 | return (pdu->data[0] & 0xfc) >> 2; |
| 439 | } |
| 440 | |
| 441 | static u8 nfc_llcp_ptype(struct sk_buff *pdu) |
| 442 | { |
| 443 | return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); |
| 444 | } |
| 445 | |
| 446 | static u8 nfc_llcp_ssap(struct sk_buff *pdu) |
| 447 | { |
| 448 | return pdu->data[1] & 0x3f; |
| 449 | } |
| 450 | |
| 451 | static u8 nfc_llcp_ns(struct sk_buff *pdu) |
| 452 | { |
| 453 | return pdu->data[2] >> 4; |
| 454 | } |
| 455 | |
| 456 | static u8 nfc_llcp_nr(struct sk_buff *pdu) |
| 457 | { |
| 458 | return pdu->data[2] & 0xf; |
| 459 | } |
| 460 | |
| 461 | static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) |
| 462 | { |
Samuel Ortiz | 279cf17 | 2012-04-10 19:43:14 +0200 | [diff] [blame] | 463 | pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 464 | sock->send_n = (sock->send_n + 1) % 16; |
| 465 | sock->recv_ack_n = (sock->recv_n - 1) % 16; |
| 466 | } |
| 467 | |
| 468 | static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 469 | u8 ssap, u8 dsap) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 470 | { |
| 471 | struct nfc_llcp_sock *sock, *llcp_sock, *n; |
| 472 | |
Samuel Ortiz | ff353d8 | 2012-05-07 12:31:19 +0200 | [diff] [blame] | 473 | pr_debug("ssap dsap %d %d\n", ssap, dsap); |
| 474 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 475 | if (ssap == 0 && dsap == 0) |
| 476 | return NULL; |
| 477 | |
| 478 | mutex_lock(&local->socket_lock); |
| 479 | sock = local->sockets[ssap]; |
| 480 | if (sock == NULL) { |
| 481 | mutex_unlock(&local->socket_lock); |
| 482 | return NULL; |
| 483 | } |
| 484 | |
| 485 | pr_debug("root dsap %d (%d)\n", sock->dsap, dsap); |
| 486 | |
| 487 | if (sock->dsap == dsap) { |
| 488 | sock_hold(&sock->sk); |
| 489 | mutex_unlock(&local->socket_lock); |
| 490 | return sock; |
| 491 | } |
| 492 | |
| 493 | list_for_each_entry_safe(llcp_sock, n, &sock->list, list) { |
| 494 | pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 495 | &llcp_sock->sk, llcp_sock->dsap); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 496 | if (llcp_sock->dsap == dsap) { |
| 497 | sock_hold(&llcp_sock->sk); |
| 498 | mutex_unlock(&local->socket_lock); |
| 499 | return llcp_sock; |
| 500 | } |
| 501 | } |
| 502 | |
| 503 | pr_err("Could not find socket for %d %d\n", ssap, dsap); |
| 504 | |
| 505 | mutex_unlock(&local->socket_lock); |
| 506 | |
| 507 | return NULL; |
| 508 | } |
| 509 | |
| 510 | static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) |
| 511 | { |
| 512 | sock_put(&sock->sk); |
| 513 | } |
| 514 | |
| 515 | static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) |
| 516 | { |
| 517 | u8 *tlv = &skb->data[2], type, length; |
| 518 | size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; |
| 519 | |
| 520 | while (offset < tlv_array_len) { |
| 521 | type = tlv[0]; |
| 522 | length = tlv[1]; |
| 523 | |
| 524 | pr_debug("type 0x%x length %d\n", type, length); |
| 525 | |
| 526 | if (type == LLCP_TLV_SN) { |
| 527 | *sn_len = length; |
| 528 | return &tlv[2]; |
| 529 | } |
| 530 | |
| 531 | offset += length + 2; |
| 532 | tlv += length + 2; |
| 533 | } |
| 534 | |
| 535 | return NULL; |
| 536 | } |
| 537 | |
| 538 | static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 539 | struct sk_buff *skb) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 540 | { |
| 541 | struct sock *new_sk, *parent; |
| 542 | struct nfc_llcp_sock *sock, *new_sock; |
| 543 | u8 dsap, ssap, bound_sap, reason; |
| 544 | |
| 545 | dsap = nfc_llcp_dsap(skb); |
| 546 | ssap = nfc_llcp_ssap(skb); |
| 547 | |
| 548 | pr_debug("%d %d\n", dsap, ssap); |
| 549 | |
| 550 | nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 551 | skb->len - LLCP_HEADER_SIZE); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 552 | |
| 553 | if (dsap != LLCP_SAP_SDP) { |
| 554 | bound_sap = dsap; |
| 555 | |
| 556 | mutex_lock(&local->socket_lock); |
| 557 | sock = local->sockets[dsap]; |
| 558 | if (sock == NULL) { |
| 559 | mutex_unlock(&local->socket_lock); |
| 560 | reason = LLCP_DM_NOBOUND; |
| 561 | goto fail; |
| 562 | } |
| 563 | |
| 564 | sock_hold(&sock->sk); |
| 565 | mutex_unlock(&local->socket_lock); |
| 566 | |
| 567 | lock_sock(&sock->sk); |
| 568 | |
| 569 | if (sock->dsap == LLCP_SAP_SDP && |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 570 | sock->sk.sk_state == LLCP_LISTEN) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 571 | goto enqueue; |
| 572 | } else { |
| 573 | u8 *sn; |
| 574 | size_t sn_len; |
| 575 | |
| 576 | sn = nfc_llcp_connect_sn(skb, &sn_len); |
| 577 | if (sn == NULL) { |
| 578 | reason = LLCP_DM_NOBOUND; |
| 579 | goto fail; |
| 580 | } |
| 581 | |
| 582 | pr_debug("Service name length %zu\n", sn_len); |
| 583 | |
| 584 | mutex_lock(&local->socket_lock); |
| 585 | for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET; |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 586 | bound_sap++) { |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 587 | sock = local->sockets[bound_sap]; |
| 588 | if (sock == NULL) |
| 589 | continue; |
| 590 | |
| 591 | if (sock->service_name == NULL || |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 592 | sock->service_name_len == 0) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 593 | continue; |
| 594 | |
| 595 | if (sock->service_name_len != sn_len) |
| 596 | continue; |
| 597 | |
| 598 | if (sock->dsap == LLCP_SAP_SDP && |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 599 | sock->sk.sk_state == LLCP_LISTEN && |
| 600 | !memcmp(sn, sock->service_name, sn_len)) { |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 601 | pr_debug("Found service name at SAP %d\n", |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 602 | bound_sap); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 603 | sock_hold(&sock->sk); |
| 604 | mutex_unlock(&local->socket_lock); |
| 605 | |
| 606 | lock_sock(&sock->sk); |
| 607 | |
| 608 | goto enqueue; |
| 609 | } |
| 610 | } |
Dan Carpenter | 341ee43 | 2011-12-16 23:25:29 +0300 | [diff] [blame] | 611 | mutex_unlock(&local->socket_lock); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 612 | } |
| 613 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 614 | reason = LLCP_DM_NOBOUND; |
| 615 | goto fail; |
| 616 | |
| 617 | enqueue: |
| 618 | parent = &sock->sk; |
| 619 | |
| 620 | if (sk_acceptq_is_full(parent)) { |
| 621 | reason = LLCP_DM_REJ; |
| 622 | release_sock(&sock->sk); |
| 623 | sock_put(&sock->sk); |
| 624 | goto fail; |
| 625 | } |
| 626 | |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 627 | new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 628 | if (new_sk == NULL) { |
| 629 | reason = LLCP_DM_REJ; |
| 630 | release_sock(&sock->sk); |
| 631 | sock_put(&sock->sk); |
| 632 | goto fail; |
| 633 | } |
| 634 | |
| 635 | new_sock = nfc_llcp_sock(new_sk); |
| 636 | new_sock->dev = local->dev; |
Samuel Ortiz | c7aa122 | 2012-05-04 11:24:16 +0200 | [diff] [blame^] | 637 | new_sock->local = nfc_llcp_local_get(local); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 638 | new_sock->nfc_protocol = sock->nfc_protocol; |
| 639 | new_sock->ssap = bound_sap; |
| 640 | new_sock->dsap = ssap; |
| 641 | new_sock->parent = parent; |
| 642 | |
| 643 | pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); |
| 644 | |
| 645 | list_add_tail(&new_sock->list, &sock->list); |
| 646 | |
| 647 | nfc_llcp_accept_enqueue(&sock->sk, new_sk); |
| 648 | |
| 649 | nfc_get_device(local->dev->idx); |
| 650 | |
| 651 | new_sk->sk_state = LLCP_CONNECTED; |
| 652 | |
| 653 | /* Wake the listening processes */ |
| 654 | parent->sk_data_ready(parent, 0); |
| 655 | |
| 656 | /* Send CC */ |
| 657 | nfc_llcp_send_cc(new_sock); |
| 658 | |
| 659 | release_sock(&sock->sk); |
| 660 | sock_put(&sock->sk); |
| 661 | |
| 662 | return; |
| 663 | |
| 664 | fail: |
| 665 | /* Send DM */ |
| 666 | nfc_llcp_send_dm(local, dsap, ssap, reason); |
| 667 | |
| 668 | return; |
| 669 | |
| 670 | } |
| 671 | |
Samuel Ortiz | d094afa | 2012-03-05 01:03:42 +0100 | [diff] [blame] | 672 | int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 673 | { |
Samuel Ortiz | d094afa | 2012-03-05 01:03:42 +0100 | [diff] [blame] | 674 | int nr_frames = 0; |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 675 | struct nfc_llcp_local *local = sock->local; |
| 676 | |
| 677 | pr_debug("Remote ready %d tx queue len %d remote rw %d", |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 678 | sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), |
| 679 | local->remote_rw); |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 680 | |
| 681 | /* Try to queue some I frames for transmission */ |
| 682 | while (sock->remote_ready && |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 683 | skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) { |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 684 | struct sk_buff *pdu, *pending_pdu; |
| 685 | |
| 686 | pdu = skb_dequeue(&sock->tx_queue); |
| 687 | if (pdu == NULL) |
| 688 | break; |
| 689 | |
| 690 | /* Update N(S)/N(R) */ |
| 691 | nfc_llcp_set_nrns(sock, pdu); |
| 692 | |
| 693 | pending_pdu = skb_clone(pdu, GFP_KERNEL); |
| 694 | |
| 695 | skb_queue_tail(&local->tx_queue, pdu); |
| 696 | skb_queue_tail(&sock->tx_pending_queue, pending_pdu); |
Samuel Ortiz | d094afa | 2012-03-05 01:03:42 +0100 | [diff] [blame] | 697 | nr_frames++; |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 698 | } |
Samuel Ortiz | d094afa | 2012-03-05 01:03:42 +0100 | [diff] [blame] | 699 | |
| 700 | return nr_frames; |
Samuel Ortiz | 4722d2b | 2012-03-05 01:03:35 +0100 | [diff] [blame] | 701 | } |
| 702 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 703 | static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 704 | struct sk_buff *skb) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 705 | { |
| 706 | struct nfc_llcp_sock *llcp_sock; |
| 707 | struct sock *sk; |
| 708 | u8 dsap, ssap, ptype, ns, nr; |
| 709 | |
| 710 | ptype = nfc_llcp_ptype(skb); |
| 711 | dsap = nfc_llcp_dsap(skb); |
| 712 | ssap = nfc_llcp_ssap(skb); |
| 713 | ns = nfc_llcp_ns(skb); |
| 714 | nr = nfc_llcp_nr(skb); |
| 715 | |
| 716 | pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); |
| 717 | |
| 718 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); |
| 719 | if (llcp_sock == NULL) { |
| 720 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); |
| 721 | return; |
| 722 | } |
| 723 | |
| 724 | sk = &llcp_sock->sk; |
| 725 | lock_sock(sk); |
| 726 | if (sk->sk_state == LLCP_CLOSED) { |
| 727 | release_sock(sk); |
| 728 | nfc_llcp_sock_put(llcp_sock); |
| 729 | } |
| 730 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 731 | /* Pass the payload upstream */ |
| 732 | if (ptype == LLCP_PDU_I) { |
| 733 | pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); |
| 734 | |
Samuel Ortiz | 53aef92 | 2012-03-05 01:03:36 +0100 | [diff] [blame] | 735 | if (ns == llcp_sock->recv_n) |
| 736 | llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; |
| 737 | else |
| 738 | pr_err("Received out of sequence I PDU\n"); |
| 739 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 740 | skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); |
| 741 | if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { |
| 742 | pr_err("receive queue is full\n"); |
| 743 | skb_queue_head(&llcp_sock->tx_backlog_queue, skb); |
| 744 | } |
| 745 | } |
| 746 | |
| 747 | /* Remove skbs from the pending queue */ |
| 748 | if (llcp_sock->send_ack_n != nr) { |
| 749 | struct sk_buff *s, *tmp; |
| 750 | |
| 751 | llcp_sock->send_ack_n = nr; |
| 752 | |
| 753 | skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) |
| 754 | if (nfc_llcp_ns(s) <= nr) { |
| 755 | skb_unlink(s, &llcp_sock->tx_pending_queue); |
| 756 | kfree_skb(s); |
| 757 | } |
| 758 | } |
| 759 | |
Samuel Ortiz | 53aef92 | 2012-03-05 01:03:36 +0100 | [diff] [blame] | 760 | if (ptype == LLCP_PDU_RR) |
| 761 | llcp_sock->remote_ready = true; |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 762 | else if (ptype == LLCP_PDU_RNR) |
Samuel Ortiz | 53aef92 | 2012-03-05 01:03:36 +0100 | [diff] [blame] | 763 | llcp_sock->remote_ready = false; |
| 764 | |
Samuel Ortiz | d094afa | 2012-03-05 01:03:42 +0100 | [diff] [blame] | 765 | if (nfc_llcp_queue_i_frames(llcp_sock) == 0) |
| 766 | nfc_llcp_send_rr(llcp_sock); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 767 | |
| 768 | release_sock(sk); |
| 769 | nfc_llcp_sock_put(llcp_sock); |
| 770 | } |
| 771 | |
| 772 | static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 773 | struct sk_buff *skb) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 774 | { |
| 775 | struct nfc_llcp_sock *llcp_sock; |
| 776 | struct sock *sk; |
| 777 | u8 dsap, ssap; |
| 778 | |
| 779 | dsap = nfc_llcp_dsap(skb); |
| 780 | ssap = nfc_llcp_ssap(skb); |
| 781 | |
| 782 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); |
| 783 | if (llcp_sock == NULL) { |
| 784 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); |
| 785 | return; |
| 786 | } |
| 787 | |
| 788 | sk = &llcp_sock->sk; |
| 789 | lock_sock(sk); |
| 790 | if (sk->sk_state == LLCP_CLOSED) { |
| 791 | release_sock(sk); |
| 792 | nfc_llcp_sock_put(llcp_sock); |
| 793 | } |
| 794 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 795 | if (sk->sk_state == LLCP_CONNECTED) { |
| 796 | nfc_put_device(local->dev); |
| 797 | sk->sk_state = LLCP_CLOSED; |
| 798 | sk->sk_state_change(sk); |
| 799 | } |
| 800 | |
| 801 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); |
| 802 | |
| 803 | release_sock(sk); |
| 804 | nfc_llcp_sock_put(llcp_sock); |
| 805 | } |
| 806 | |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 807 | static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 808 | { |
| 809 | struct nfc_llcp_sock *llcp_sock; |
Samuel Ortiz | ff353d8 | 2012-05-07 12:31:19 +0200 | [diff] [blame] | 810 | struct sock *sk; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 811 | u8 dsap, ssap; |
| 812 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 813 | dsap = nfc_llcp_dsap(skb); |
| 814 | ssap = nfc_llcp_ssap(skb); |
| 815 | |
| 816 | llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); |
| 817 | |
| 818 | if (llcp_sock == NULL) |
| 819 | llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); |
| 820 | |
| 821 | if (llcp_sock == NULL) { |
| 822 | pr_err("Invalid CC\n"); |
| 823 | nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); |
| 824 | |
| 825 | return; |
| 826 | } |
| 827 | |
| 828 | llcp_sock->dsap = ssap; |
Samuel Ortiz | ff353d8 | 2012-05-07 12:31:19 +0200 | [diff] [blame] | 829 | sk = &llcp_sock->sk; |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 830 | |
| 831 | nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 832 | skb->len - LLCP_HEADER_SIZE); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 833 | |
Samuel Ortiz | ff353d8 | 2012-05-07 12:31:19 +0200 | [diff] [blame] | 834 | sk->sk_state = LLCP_CONNECTED; |
| 835 | sk->sk_state_change(sk); |
| 836 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 837 | nfc_llcp_sock_put(llcp_sock); |
| 838 | } |
| 839 | |
| 840 | static void nfc_llcp_rx_work(struct work_struct *work) |
| 841 | { |
| 842 | struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 843 | rx_work); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 844 | u8 dsap, ssap, ptype; |
| 845 | struct sk_buff *skb; |
| 846 | |
| 847 | skb = local->rx_pending; |
| 848 | if (skb == NULL) { |
| 849 | pr_debug("No pending SKB\n"); |
| 850 | return; |
| 851 | } |
| 852 | |
| 853 | ptype = nfc_llcp_ptype(skb); |
| 854 | dsap = nfc_llcp_dsap(skb); |
| 855 | ssap = nfc_llcp_ssap(skb); |
| 856 | |
| 857 | pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); |
| 858 | |
Samuel Ortiz | 4be646e | 2012-04-10 19:43:13 +0200 | [diff] [blame] | 859 | if (ptype != LLCP_PDU_SYMM) |
| 860 | print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, |
| 861 | 16, 1, skb->data, skb->len, true); |
| 862 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 863 | switch (ptype) { |
| 864 | case LLCP_PDU_SYMM: |
| 865 | pr_debug("SYMM\n"); |
| 866 | break; |
| 867 | |
| 868 | case LLCP_PDU_CONNECT: |
| 869 | pr_debug("CONNECT\n"); |
| 870 | nfc_llcp_recv_connect(local, skb); |
| 871 | break; |
| 872 | |
| 873 | case LLCP_PDU_DISC: |
| 874 | pr_debug("DISC\n"); |
| 875 | nfc_llcp_recv_disc(local, skb); |
| 876 | break; |
| 877 | |
| 878 | case LLCP_PDU_CC: |
| 879 | pr_debug("CC\n"); |
| 880 | nfc_llcp_recv_cc(local, skb); |
| 881 | break; |
| 882 | |
| 883 | case LLCP_PDU_I: |
| 884 | case LLCP_PDU_RR: |
Samuel Ortiz | 53aef92 | 2012-03-05 01:03:36 +0100 | [diff] [blame] | 885 | case LLCP_PDU_RNR: |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 886 | pr_debug("I frame\n"); |
| 887 | nfc_llcp_recv_hdlc(local, skb); |
| 888 | break; |
| 889 | |
| 890 | } |
| 891 | |
| 892 | queue_work(local->tx_wq, &local->tx_work); |
| 893 | kfree_skb(local->rx_pending); |
| 894 | local->rx_pending = NULL; |
| 895 | |
| 896 | return; |
| 897 | } |
| 898 | |
| 899 | void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) |
| 900 | { |
| 901 | struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; |
| 902 | |
| 903 | pr_debug("Received an LLCP PDU\n"); |
| 904 | if (err < 0) { |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 905 | pr_err("err %d\n", err); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 906 | return; |
| 907 | } |
| 908 | |
| 909 | local->rx_pending = skb_get(skb); |
| 910 | del_timer(&local->link_timer); |
| 911 | queue_work(local->rx_wq, &local->rx_work); |
| 912 | |
| 913 | return; |
| 914 | } |
| 915 | |
| 916 | void nfc_llcp_mac_is_down(struct nfc_dev *dev) |
| 917 | { |
| 918 | struct nfc_llcp_local *local; |
| 919 | |
| 920 | local = nfc_llcp_find_local(dev); |
| 921 | if (local == NULL) |
| 922 | return; |
| 923 | |
Samuel Ortiz | b9a76f1 | 2012-03-05 01:03:40 +0100 | [diff] [blame] | 924 | nfc_llcp_clear_sdp(local); |
| 925 | |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 926 | /* Close and purge all existing sockets */ |
| 927 | nfc_llcp_socket_release(local); |
| 928 | } |
| 929 | |
| 930 | void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, |
| 931 | u8 comm_mode, u8 rf_mode) |
| 932 | { |
| 933 | struct nfc_llcp_local *local; |
| 934 | |
| 935 | pr_debug("rf mode %d\n", rf_mode); |
| 936 | |
| 937 | local = nfc_llcp_find_local(dev); |
| 938 | if (local == NULL) |
| 939 | return; |
| 940 | |
| 941 | local->target_idx = target_idx; |
| 942 | local->comm_mode = comm_mode; |
| 943 | local->rf_mode = rf_mode; |
| 944 | |
| 945 | if (rf_mode == NFC_RF_INITIATOR) { |
| 946 | pr_debug("Queueing Tx work\n"); |
| 947 | |
| 948 | queue_work(local->tx_wq, &local->tx_work); |
| 949 | } else { |
| 950 | mod_timer(&local->link_timer, |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 951 | jiffies + msecs_to_jiffies(local->remote_lto)); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 952 | } |
| 953 | } |
| 954 | |
| 955 | int nfc_llcp_register_device(struct nfc_dev *ndev) |
| 956 | { |
| 957 | struct device *dev = &ndev->dev; |
| 958 | struct nfc_llcp_local *local; |
| 959 | char name[32]; |
| 960 | int err; |
| 961 | |
| 962 | local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); |
| 963 | if (local == NULL) |
| 964 | return -ENOMEM; |
| 965 | |
| 966 | local->dev = ndev; |
| 967 | INIT_LIST_HEAD(&local->list); |
Samuel Ortiz | c7aa122 | 2012-05-04 11:24:16 +0200 | [diff] [blame^] | 968 | kref_init(&local->ref); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 969 | mutex_init(&local->sdp_lock); |
| 970 | mutex_init(&local->socket_lock); |
| 971 | init_timer(&local->link_timer); |
| 972 | local->link_timer.data = (unsigned long) local; |
| 973 | local->link_timer.function = nfc_llcp_symm_timer; |
| 974 | |
| 975 | skb_queue_head_init(&local->tx_queue); |
| 976 | INIT_WORK(&local->tx_work, nfc_llcp_tx_work); |
| 977 | snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 978 | local->tx_wq = |
| 979 | alloc_workqueue(name, |
| 980 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, |
| 981 | 1); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 982 | if (local->tx_wq == NULL) { |
| 983 | err = -ENOMEM; |
| 984 | goto err_local; |
| 985 | } |
| 986 | |
| 987 | local->rx_pending = NULL; |
| 988 | INIT_WORK(&local->rx_work, nfc_llcp_rx_work); |
| 989 | snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 990 | local->rx_wq = |
| 991 | alloc_workqueue(name, |
| 992 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, |
| 993 | 1); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 994 | if (local->rx_wq == NULL) { |
| 995 | err = -ENOMEM; |
| 996 | goto err_tx_wq; |
| 997 | } |
| 998 | |
| 999 | INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); |
| 1000 | snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); |
Samuel Ortiz | 427a2eb | 2012-03-05 01:03:52 +0100 | [diff] [blame] | 1001 | local->timeout_wq = |
| 1002 | alloc_workqueue(name, |
| 1003 | WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, |
| 1004 | 1); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 1005 | if (local->timeout_wq == NULL) { |
| 1006 | err = -ENOMEM; |
| 1007 | goto err_rx_wq; |
| 1008 | } |
| 1009 | |
| 1010 | nfc_llcp_build_gb(local); |
| 1011 | |
| 1012 | local->remote_miu = LLCP_DEFAULT_MIU; |
| 1013 | local->remote_lto = LLCP_DEFAULT_LTO; |
| 1014 | local->remote_rw = LLCP_DEFAULT_RW; |
| 1015 | |
| 1016 | list_add(&llcp_devices, &local->list); |
| 1017 | |
| 1018 | return 0; |
| 1019 | |
| 1020 | err_rx_wq: |
| 1021 | destroy_workqueue(local->rx_wq); |
| 1022 | |
| 1023 | err_tx_wq: |
| 1024 | destroy_workqueue(local->tx_wq); |
| 1025 | |
| 1026 | err_local: |
| 1027 | kfree(local); |
| 1028 | |
| 1029 | return 0; |
| 1030 | } |
| 1031 | |
| 1032 | void nfc_llcp_unregister_device(struct nfc_dev *dev) |
| 1033 | { |
| 1034 | struct nfc_llcp_local *local = nfc_llcp_find_local(dev); |
| 1035 | |
| 1036 | if (local == NULL) { |
| 1037 | pr_debug("No such device\n"); |
| 1038 | return; |
| 1039 | } |
| 1040 | |
Samuel Ortiz | c7aa122 | 2012-05-04 11:24:16 +0200 | [diff] [blame^] | 1041 | nfc_llcp_local_put(local); |
Samuel Ortiz | d646960 | 2011-12-14 16:43:12 +0100 | [diff] [blame] | 1042 | } |
| 1043 | |
| 1044 | int __init nfc_llcp_init(void) |
| 1045 | { |
| 1046 | INIT_LIST_HEAD(&llcp_devices); |
| 1047 | |
| 1048 | return nfc_llcp_sock_init(); |
| 1049 | } |
| 1050 | |
| 1051 | void nfc_llcp_exit(void) |
| 1052 | { |
| 1053 | nfc_llcp_sock_exit(); |
| 1054 | } |