bluetooth_finddev.c 5.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159
  1. /****************************************************************************
  2. * net/bluetooth/bluetooth_finddev.c
  3. *
  4. * Copyright (C) 2018 Gregory Nutt. All rights reserved.
  5. * Author: Gregory Nutt <gnutt@nuttx.org>
  6. *
  7. * Redistribution and use in source and binary forms, with or without
  8. * modification, are permitted provided that the following conditions
  9. * are met:
  10. *
  11. * 1. Redistributions of source code must retain the above copyright
  12. * notice, this list of conditions and the following disclaimer.
  13. * 2. Redistributions in binary form must reproduce the above copyright
  14. * notice, this list of conditions and the following disclaimer in
  15. * the documentation and/or other materials provided with the
  16. * distribution.
  17. * 3. Neither the name NuttX nor the names of its contributors may be
  18. * used to endorse or promote products derived from this software
  19. * without specific prior written permission.
  20. *
  21. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
  28. * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  29. * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32. * POSSIBILITY OF SUCH DAMAGE.
  33. *
  34. ****************************************************************************/
  35. /****************************************************************************
  36. * Included Files
  37. ****************************************************************************/
  38. #include <nuttx/config.h>
  39. #include <assert.h>
  40. #include <nuttx/net/net.h>
  41. #include <nuttx/net/radiodev.h>
  42. #include <nuttx/net/bluetooth.h>
  43. #include <nuttx/wireless/bluetooth/bt_hci.h>
  44. #include "netdev/netdev.h"
  45. #include "bluetooth/bluetooth.h"
  46. #ifdef CONFIG_NET_BLUETOOTH
  47. /****************************************************************************
  48. * Private Types
  49. ****************************************************************************/
  50. struct bluetooth_finddev_s
  51. {
  52. FAR const bt_addr_t *bf_addr;
  53. FAR struct radio_driver_s *bf_radio;
  54. };
  55. /****************************************************************************
  56. * Private Functions
  57. ****************************************************************************/
  58. /****************************************************************************
  59. * Name: bluetooth_dev_callback
  60. *
  61. * Description:
  62. * Check if this device matches the connections local address.
  63. *
  64. * Input Parameters:
  65. * dev - The next network device in the enumeration
  66. *
  67. * Returned Value:
  68. * 0 if there is no match (meaning to continue looking); 1 if there is a
  69. * match (meaning to stop the search).
  70. *
  71. ****************************************************************************/
  72. static int bluetooth_dev_callback(FAR struct net_driver_s *dev, FAR void *arg)
  73. {
  74. FAR struct bluetooth_finddev_s *match =
  75. (FAR struct bluetooth_finddev_s *)arg;
  76. DEBUGASSERT(dev != NULL && match != NULL && match->bf_addr != NULL);
  77. /* First, check if this network device is an Bluetooth radio and that
  78. * it has an assigned Bluetooth address.
  79. */
  80. if (dev->d_lltype == NET_LL_BLUETOOTH && dev->d_mac.radio.nv_addrlen > 0)
  81. {
  82. DEBUGASSERT(dev->d_mac.radio.nv_addrlen == BLUETOOTH_ADDRSIZE);
  83. /* Does the device address match */
  84. if (BLUETOOTH_ADDRCMP(dev->d_mac.radio.nv_addr, match->bf_addr))
  85. {
  86. /* Yes.. save the match and return 1 to stop the search */
  87. match->bf_radio = (FAR struct radio_driver_s *)dev;
  88. return 1;
  89. }
  90. }
  91. /* Keep looking */
  92. return 0;
  93. }
  94. /****************************************************************************
  95. * Public Functions
  96. ****************************************************************************/
  97. /****************************************************************************
  98. * Name: bluetooth_find_device
  99. *
  100. * Description:
  101. * Select the network driver to use with the Bluetooth transaction.
  102. *
  103. * Input Parameters:
  104. * conn - Bluetooth connection structure (not currently used).
  105. * addr - The address to match the devices assigned address
  106. *
  107. * Returned Value:
  108. * A pointer to the network driver to use. NULL is returned on any
  109. * failure.
  110. *
  111. ****************************************************************************/
  112. FAR struct radio_driver_s *
  113. bluetooth_find_device(FAR struct bluetooth_conn_s *conn,
  114. FAR const bt_addr_t *addr)
  115. {
  116. struct bluetooth_finddev_s match;
  117. int ret;
  118. DEBUGASSERT(conn != NULL);
  119. match.bf_addr = addr;
  120. match.bf_radio = NULL;
  121. /* Search for the Bluetooth network device whose MAC is equal to the
  122. * sockets bound local address.
  123. */
  124. ret = netdev_foreach(bluetooth_dev_callback, (FAR void *)&match);
  125. if (ret == 1)
  126. {
  127. DEBUGASSERT(match.bf_radio != NULL);
  128. return match.bf_radio;
  129. }
  130. return NULL;
  131. }
  132. #endif /* CONFIG_NET_BLUETOOTH */