diff --git a/usr.sbin/bluetooth/bthidcontrol/sdp.c b/usr.sbin/bluetooth/bthidcontrol/sdp.c index dc83229f080c..4754744a866a 100644 --- a/usr.sbin/bluetooth/bthidcontrol/sdp.c +++ b/usr.sbin/bluetooth/bthidcontrol/sdp.c @@ -1,503 +1,503 @@ /*- * sdp.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: sdp.c,v 1.3 2004/02/17 22:14:57 max Exp $ */ -#include +#include #include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include #include #include #include #include "bthid_config.h" #include "bthidcontrol.h" static int32_t hid_sdp_query (bdaddr_t const *local, struct hid_device *hd, int32_t *error); static int32_t hid_sdp_parse_protocol_descriptor_list (sdp_attr_p a); static int32_t hid_sdp_parse_hid_descriptor (sdp_attr_p a); static int32_t hid_sdp_parse_boolean (sdp_attr_p a); /* * Hard coded attribute IDs taken from the * DEVICE IDENTIFICATION PROFILE SPECIFICATION V13 p.12 */ #define SDP_ATTR_DEVICE_ID_SERVICE_VENDORID 0x0201 #define SDP_ATTR_DEVICE_ID_SERVICE_PRODUCTID 0x0202 #define SDP_ATTR_DEVICE_ID_SERVICE_VERSION 0x0203 #define SDP_ATTR_DEVICE_ID_RANGE SDP_ATTR_RANGE( \ SDP_ATTR_DEVICE_ID_SERVICE_VENDORID, SDP_ATTR_DEVICE_ID_SERVICE_VERSION ) static uint16_t service = SDP_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE; static uint16_t service_devid = SDP_SERVICE_CLASS_PNP_INFORMATION; static uint32_t attrs_devid = SDP_ATTR_DEVICE_ID_RANGE; static uint32_t attrs[] = { SDP_ATTR_RANGE( SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST, SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST), SDP_ATTR_RANGE (SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS, SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS), SDP_ATTR_RANGE( 0x0205, /* HIDReconnectInitiate */ 0x0205), SDP_ATTR_RANGE( 0x0206, /* HIDDescriptorList */ 0x0206), SDP_ATTR_RANGE( 0x0209, /* HIDBatteryPower */ 0x0209), SDP_ATTR_RANGE( 0x020d, /* HIDNormallyConnectable */ 0x020d) }; -#define nattrs (sizeof(attrs)/sizeof(attrs[0])) +#define nattrs nitems(attrs) static sdp_attr_t values[8]; -#define nvalues (sizeof(values)/sizeof(values[0])) +#define nvalues nitems(values) static uint8_t buffer[nvalues][512]; /* * Query remote device */ #undef hid_sdp_query_exit #define hid_sdp_query_exit(e) { \ if (error != NULL) \ *error = (e); \ if (ss != NULL) { \ sdp_close(ss); \ ss = NULL; \ } \ return (((e) == 0)? 0 : -1); \ } static void hid_init_return_values() { int i; for (i = 0; i < nvalues; i ++) { values[i].flags = SDP_ATTR_INVALID; values[i].attr = 0; values[i].vlen = sizeof(buffer[i]); values[i].value = buffer[i]; } } static int32_t hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error) { void *ss = NULL; uint8_t *hid_descriptor = NULL, *v; int32_t i, control_psm = -1, interrupt_psm = -1, reconnect_initiate = -1, normally_connectable = 0, battery_power = 0, hid_descriptor_length = -1, type; int16_t vendor_id = 0, product_id = 0, version = 0; bdaddr_t sdp_local; char devname[HCI_DEVNAME_SIZE]; if (local == NULL) local = NG_HCI_BDADDR_ANY; if (hd == NULL) hid_sdp_query_exit(EINVAL); hid_init_return_values(); if ((ss = sdp_open(local, &hd->bdaddr)) == NULL) hid_sdp_query_exit(ENOMEM); if (sdp_error(ss) != 0) hid_sdp_query_exit(sdp_error(ss)); if (sdp_search(ss, 1, &service, nattrs, attrs, nvalues, values) != 0) hid_sdp_query_exit(sdp_error(ss)); for (i = 0; i < nvalues; i ++) { if (values[i].flags != SDP_ATTR_OK) continue; switch (values[i].attr) { case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST: control_psm = hid_sdp_parse_protocol_descriptor_list(&values[i]); break; case SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS: interrupt_psm = hid_sdp_parse_protocol_descriptor_list(&values[i]); break; case 0x0205: /* HIDReconnectInitiate */ reconnect_initiate = hid_sdp_parse_boolean(&values[i]); break; case 0x0206: /* HIDDescriptorList */ if (hid_sdp_parse_hid_descriptor(&values[i]) == 0) { hid_descriptor = values[i].value; hid_descriptor_length = values[i].vlen; } break; case 0x0209: /* HIDBatteryPower */ battery_power = hid_sdp_parse_boolean(&values[i]); break; case 0x020d: /* HIDNormallyConnectable */ normally_connectable = hid_sdp_parse_boolean(&values[i]); break; } } hid_init_return_values(); if (sdp_search(ss, 1, &service_devid, 1, &attrs_devid, nvalues, values) != 0) hid_sdp_query_exit(sdp_error(ss)); /* Try extract HCI bdaddr from opened SDP session */ if (sdp_get_lcaddr(ss, &sdp_local) != 0 || bt_devname(devname, &sdp_local) == 0) hid_sdp_query_exit(ENOATTR); sdp_close(ss); ss = NULL; /* If search is successful, scan through return vals */ for (i = 0; i < 3; i ++ ) { if (values[i].flags == SDP_ATTR_INVALID ) continue; /* Expecting tag + uint16_t on all 3 attributes */ if (values[i].vlen != 3) continue; /* Make sure, we're reading a uint16_t */ v = values[i].value; SDP_GET8(type, v); if (type != SDP_DATA_UINT16 ) continue; switch (values[i].attr) { case SDP_ATTR_DEVICE_ID_SERVICE_VENDORID: SDP_GET16(vendor_id, v); break; case SDP_ATTR_DEVICE_ID_SERVICE_PRODUCTID: SDP_GET16(product_id, v); break; case SDP_ATTR_DEVICE_ID_SERVICE_VERSION: SDP_GET16(version, v); break; default: break; } } if (control_psm == -1 || interrupt_psm == -1 || reconnect_initiate == -1 || hid_descriptor == NULL || hid_descriptor_length == -1) hid_sdp_query_exit(ENOATTR); hd->name = bt_devremote_name_gen(devname, &hd->bdaddr); hd->vendor_id = vendor_id; hd->product_id = product_id; hd->version = version; hd->control_psm = control_psm; hd->interrupt_psm = interrupt_psm; hd->reconnect_initiate = reconnect_initiate? 1 : 0; hd->battery_power = battery_power? 1 : 0; hd->normally_connectable = normally_connectable? 1 : 0; hd->desc = hid_use_report_desc(hid_descriptor, hid_descriptor_length); if (hd->desc == NULL) hid_sdp_query_exit(ENOMEM); return (0); } /* * seq len 2 * seq len 2 * uuid value 3 * uint16 value 3 * seq len 2 * uuid value 3 */ static int32_t hid_sdp_parse_protocol_descriptor_list(sdp_attr_p a) { uint8_t *ptr = a->value; uint8_t *end = a->value + a->vlen; int32_t type, len, uuid, psm; if (end - ptr < 15) return (-1); if (a->attr == SDP_ATTR_ADDITIONAL_PROTOCOL_DESCRIPTOR_LISTS) { SDP_GET8(type, ptr); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, ptr); break; case SDP_DATA_SEQ16: SDP_GET16(len, ptr); break; case SDP_DATA_SEQ32: SDP_GET32(len, ptr); break; default: return (-1); } if (ptr + len > end) return (-1); } SDP_GET8(type, ptr); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, ptr); break; case SDP_DATA_SEQ16: SDP_GET16(len, ptr); break; case SDP_DATA_SEQ32: SDP_GET32(len, ptr); break; default: return (-1); } if (ptr + len > end) return (-1); /* Protocol */ SDP_GET8(type, ptr); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, ptr); break; case SDP_DATA_SEQ16: SDP_GET16(len, ptr); break; case SDP_DATA_SEQ32: SDP_GET32(len, ptr); break; default: return (-1); } if (ptr + len > end) return (-1); /* UUID */ if (ptr + 3 > end) return (-1); SDP_GET8(type, ptr); switch (type) { case SDP_DATA_UUID16: SDP_GET16(uuid, ptr); if (uuid != SDP_UUID_PROTOCOL_L2CAP) return (-1); break; case SDP_DATA_UUID32: /* XXX FIXME can we have 32-bit UUID */ case SDP_DATA_UUID128: /* XXX FIXME can we have 128-bit UUID */ default: return (-1); } /* PSM */ if (ptr + 3 > end) return (-1); SDP_GET8(type, ptr); if (type != SDP_DATA_UINT16) return (-1); SDP_GET16(psm, ptr); return (psm); } /* * seq len 2 * seq len 2 * uint8 value8 2 * str value 3 */ static int32_t hid_sdp_parse_hid_descriptor(sdp_attr_p a) { uint8_t *ptr = a->value; uint8_t *end = a->value + a->vlen; int32_t type, len, descriptor_type; if (end - ptr < 9) return (-1); SDP_GET8(type, ptr); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, ptr); break; case SDP_DATA_SEQ16: SDP_GET16(len, ptr); break; case SDP_DATA_SEQ32: SDP_GET32(len, ptr); break; default: return (-1); } if (ptr + len > end) return (-1); while (ptr < end) { /* Descriptor */ SDP_GET8(type, ptr); switch (type) { case SDP_DATA_SEQ8: if (ptr + 1 > end) return (-1); SDP_GET8(len, ptr); break; case SDP_DATA_SEQ16: if (ptr + 2 > end) return (-1); SDP_GET16(len, ptr); break; case SDP_DATA_SEQ32: if (ptr + 4 > end) return (-1); SDP_GET32(len, ptr); break; default: return (-1); } /* Descripor type */ if (ptr + 1 > end) return (-1); SDP_GET8(type, ptr); if (type != SDP_DATA_UINT8 || ptr + 1 > end) return (-1); SDP_GET8(descriptor_type, ptr); /* Descriptor value */ if (ptr + 1 > end) return (-1); SDP_GET8(type, ptr); switch (type) { case SDP_DATA_STR8: if (ptr + 1 > end) return (-1); SDP_GET8(len, ptr); break; case SDP_DATA_STR16: if (ptr + 2 > end) return (-1); SDP_GET16(len, ptr); break; case SDP_DATA_STR32: if (ptr + 4 > end) return (-1); SDP_GET32(len, ptr); break; default: return (-1); } if (ptr + len > end) return (-1); if (descriptor_type == UDESC_REPORT && len > 0) { a->value = ptr; a->vlen = len; return (0); } ptr += len; } return (-1); } /* bool8 int8 */ static int32_t hid_sdp_parse_boolean(sdp_attr_p a) { if (a->vlen != 2 || a->value[0] != SDP_DATA_BOOL) return (-1); return (a->value[1]); } /* Perform SDP query */ static int32_t hid_query(bdaddr_t *bdaddr, int argc, char **argv) { struct hid_device hd; int e; memcpy(&hd.bdaddr, bdaddr, sizeof(hd.bdaddr)); if (hid_sdp_query(NULL, &hd, &e) < 0) { fprintf(stderr, "Could not perform SDP query on the " \ "device %s. %s (%d)\n", bt_ntoa(bdaddr, NULL), strerror(e), e); return (FAILED); } print_hid_device(&hd, stdout); return (OK); } struct bthid_command sdp_commands[] = { { "Query", "Perform SDP query to the specified device and print HID configuration entry\n"\ "for the device. The configuration entry should be appended to the Bluetooth\n"\ "HID daemon configuration file and the daemon should be restarted.\n", hid_query }, { NULL, NULL, NULL } }; diff --git a/usr.sbin/bluetooth/bthidd/kbd.c b/usr.sbin/bluetooth/bthidd/kbd.c index c1616c7e4bd3..a807a9b27dec 100644 --- a/usr.sbin/bluetooth/bthidd/kbd.c +++ b/usr.sbin/bluetooth/bthidd/kbd.c @@ -1,613 +1,614 @@ /* * kbd.c */ /*- * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2006 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: kbd.c,v 1.4 2006/09/07 21:06:53 max Exp $ */ #include #include #include +#include #include #include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "bthid_config.h" #include "bthidd.h" #include "btuinput.h" #include "kbd.h" static void kbd_write(bitstr_t *m, int32_t fb, int32_t make, int32_t fd); static int32_t kbd_xlate(int32_t code, int32_t make, int32_t *b, int32_t const *eob); static void uinput_kbd_write(bitstr_t *m, int32_t fb, int32_t make, int32_t fd); /* * HID code to PS/2 set 1 code translation table. * * http://www.microsoft.com/whdc/device/input/Scancode.mspx * * The table only contains "make" (key pressed) codes. * The "break" (key released) code is generated as "make" | 0x80 */ #define E0PREFIX (1U << 31) #define NOBREAK (1 << 30) #define CODEMASK (~(E0PREFIX|NOBREAK)) static int32_t const x[] = { /*==================================================*/ /* Name HID code Make Break*/ /*==================================================*/ /* No Event 00 */ -1, /* None */ /* Overrun Error 01 */ NOBREAK|0xFF, /* None */ /* POST Fail 02 */ NOBREAK|0xFC, /* None */ /* ErrorUndefined 03 */ -1, /* Unassigned */ /* a A 04 */ 0x1E, /* 9E */ /* b B 05 */ 0x30, /* B0 */ /* c C 06 */ 0x2E, /* AE */ /* d D 07 */ 0x20, /* A0 */ /* e E 08 */ 0x12, /* 92 */ /* f F 09 */ 0x21, /* A1 */ /* g G 0A */ 0x22, /* A2 */ /* h H 0B */ 0x23, /* A3 */ /* i I 0C */ 0x17, /* 97 */ /* j J 0D */ 0x24, /* A4 */ /* k K 0E */ 0x25, /* A5 */ /* l L 0F */ 0x26, /* A6 */ /* m M 10 */ 0x32, /* B2 */ /* n N 11 */ 0x31, /* B1 */ /* o O 12 */ 0x18, /* 98 */ /* p P 13 */ 0x19, /* 99 */ /* q Q 14 */ 0x10, /* 90 */ /* r R 15 */ 0x13, /* 93 */ /* s S 16 */ 0x1F, /* 9F */ /* t T 17 */ 0x14, /* 94 */ /* u U 18 */ 0x16, /* 96 */ /* v V 19 */ 0x2F, /* AF */ /* w W 1A */ 0x11, /* 91 */ /* x X 1B */ 0x2D, /* AD */ /* y Y 1C */ 0x15, /* 95 */ /* z Z 1D */ 0x2C, /* AC */ /* 1 ! 1E */ 0x02, /* 82 */ /* 2 @ 1F */ 0x03, /* 83 */ /* 3 # 20 */ 0x04, /* 84 */ /* 4 $ 21 */ 0x05, /* 85 */ /* 5 % 22 */ 0x06, /* 86 */ /* 6 ^ 23 */ 0x07, /* 87 */ /* 7 & 24 */ 0x08, /* 88 */ /* 8 * 25 */ 0x09, /* 89 */ /* 9 ( 26 */ 0x0A, /* 8A */ /* 0 ) 27 */ 0x0B, /* 8B */ /* Return 28 */ 0x1C, /* 9C */ /* Escape 29 */ 0x01, /* 81 */ /* Backspace 2A */ 0x0E, /* 8E */ /* Tab 2B */ 0x0F, /* 8F */ /* Space 2C */ 0x39, /* B9 */ /* - _ 2D */ 0x0C, /* 8C */ /* = + 2E */ 0x0D, /* 8D */ /* [ { 2F */ 0x1A, /* 9A */ /* ] } 30 */ 0x1B, /* 9B */ /* \ | 31 */ 0x2B, /* AB */ /* Europe 1 32 */ 0x2B, /* AB */ /* ; : 33 */ 0x27, /* A7 */ /* " ' 34 */ 0x28, /* A8 */ /* ` ~ 35 */ 0x29, /* A9 */ /* comma < 36 */ 0x33, /* B3 */ /* . > 37 */ 0x34, /* B4 */ /* / ? 38 */ 0x35, /* B5 */ /* Caps Lock 39 */ 0x3A, /* BA */ /* F1 3A */ 0x3B, /* BB */ /* F2 3B */ 0x3C, /* BC */ /* F3 3C */ 0x3D, /* BD */ /* F4 3D */ 0x3E, /* BE */ /* F5 3E */ 0x3F, /* BF */ /* F6 3F */ 0x40, /* C0 */ /* F7 40 */ 0x41, /* C1 */ /* F8 41 */ 0x42, /* C2 */ /* F9 42 */ 0x43, /* C3 */ /* F10 43 */ 0x44, /* C4 */ /* F11 44 */ 0x57, /* D7 */ /* F12 45 */ 0x58, /* D8 */ /* Print Screen 46 */ E0PREFIX|0x37, /* E0 B7 */ /* Scroll Lock 47 */ 0x46, /* C6 */ #if 0 /* Break (Ctrl-Pause) 48 */ E0 46 E0 C6, /* None */ /* Pause 48 */ E1 1D 45 E1 9D C5, /* None */ #else /* Break (Ctrl-Pause)/Pause 48 */ NOBREAK /* Special case */, /* None */ #endif /* Insert 49 */ E0PREFIX|0x52, /* E0 D2 */ /* Home 4A */ E0PREFIX|0x47, /* E0 C7 */ /* Page Up 4B */ E0PREFIX|0x49, /* E0 C9 */ /* Delete 4C */ E0PREFIX|0x53, /* E0 D3 */ /* End 4D */ E0PREFIX|0x4F, /* E0 CF */ /* Page Down 4E */ E0PREFIX|0x51, /* E0 D1 */ /* Right Arrow 4F */ E0PREFIX|0x4D, /* E0 CD */ /* Left Arrow 50 */ E0PREFIX|0x4B, /* E0 CB */ /* Down Arrow 51 */ E0PREFIX|0x50, /* E0 D0 */ /* Up Arrow 52 */ E0PREFIX|0x48, /* E0 C8 */ /* Num Lock 53 */ 0x45, /* C5 */ /* Keypad / 54 */ E0PREFIX|0x35, /* E0 B5 */ /* Keypad * 55 */ 0x37, /* B7 */ /* Keypad - 56 */ 0x4A, /* CA */ /* Keypad + 57 */ 0x4E, /* CE */ /* Keypad Enter 58 */ E0PREFIX|0x1C, /* E0 9C */ /* Keypad 1 End 59 */ 0x4F, /* CF */ /* Keypad 2 Down 5A */ 0x50, /* D0 */ /* Keypad 3 PageDn 5B */ 0x51, /* D1 */ /* Keypad 4 Left 5C */ 0x4B, /* CB */ /* Keypad 5 5D */ 0x4C, /* CC */ /* Keypad 6 Right 5E */ 0x4D, /* CD */ /* Keypad 7 Home 5F */ 0x47, /* C7 */ /* Keypad 8 Up 60 */ 0x48, /* C8 */ /* Keypad 9 PageUp 61 */ 0x49, /* C9 */ /* Keypad 0 Insert 62 */ 0x52, /* D2 */ /* Keypad . Delete 63 */ 0x53, /* D3 */ /* Europe 2 64 */ 0x56, /* D6 */ /* App 65 */ E0PREFIX|0x5D, /* E0 DD */ /* Keyboard Power 66 */ E0PREFIX|0x5E, /* E0 DE */ /* Keypad = 67 */ 0x59, /* D9 */ /* F13 68 */ 0x64, /* E4 */ /* F14 69 */ 0x65, /* E5 */ /* F15 6A */ 0x66, /* E6 */ /* F16 6B */ 0x67, /* E7 */ /* F17 6C */ 0x68, /* E8 */ /* F18 6D */ 0x69, /* E9 */ /* F19 6E */ 0x6A, /* EA */ /* F20 6F */ 0x6B, /* EB */ /* F21 70 */ 0x6C, /* EC */ /* F22 71 */ 0x6D, /* ED */ /* F23 72 */ 0x6E, /* EE */ /* F24 73 */ 0x76, /* F6 */ /* Keyboard Execute 74 */ -1, /* Unassigned */ /* Keyboard Help 75 */ -1, /* Unassigned */ /* Keyboard Menu 76 */ -1, /* Unassigned */ /* Keyboard Select 77 */ -1, /* Unassigned */ /* Keyboard Stop 78 */ -1, /* Unassigned */ /* Keyboard Again 79 */ -1, /* Unassigned */ /* Keyboard Undo 7A */ -1, /* Unassigned */ /* Keyboard Cut 7B */ -1, /* Unassigned */ /* Keyboard Copy 7C */ -1, /* Unassigned */ /* Keyboard Paste 7D */ -1, /* Unassigned */ /* Keyboard Find 7E */ -1, /* Unassigned */ /* Keyboard Mute 7F */ -1, /* Unassigned */ /* Keyboard Volume Up 80 */ -1, /* Unassigned */ /* Keyboard Volume Dn 81 */ -1, /* Unassigned */ /* Keyboard Locking Caps Lock 82 */ -1, /* Unassigned */ /* Keyboard Locking Num Lock 83 */ -1, /* Unassigned */ /* Keyboard Locking Scroll Lock 84 */ -1, /* Unassigned */ /* Keypad comma 85 */ 0x7E, /* FE */ /* Keyboard Equal Sign 86 */ -1, /* Unassigned */ /* Keyboard Int'l 1 87 */ 0x73, /* F3 */ /* Keyboard Int'l 2 88 */ 0x70, /* F0 */ /* Keyboard Int'l 2 89 */ 0x7D, /* FD */ /* Keyboard Int'l 4 8A */ 0x79, /* F9 */ /* Keyboard Int'l 5 8B */ 0x7B, /* FB */ /* Keyboard Int'l 6 8C */ 0x5C, /* DC */ /* Keyboard Int'l 7 8D */ -1, /* Unassigned */ /* Keyboard Int'l 8 8E */ -1, /* Unassigned */ /* Keyboard Int'l 9 8F */ -1, /* Unassigned */ /* Keyboard Lang 1 90 */ 0x71, /* Kana */ /* Keyboard Lang 2 91 */ 0x72, /* Eisu */ /* Keyboard Lang 3 92 */ 0x78, /* F8 */ /* Keyboard Lang 4 93 */ 0x77, /* F7 */ /* Keyboard Lang 5 94 */ 0x76, /* F6 */ /* Keyboard Lang 6 95 */ -1, /* Unassigned */ /* Keyboard Lang 7 96 */ -1, /* Unassigned */ /* Keyboard Lang 8 97 */ -1, /* Unassigned */ /* Keyboard Lang 9 98 */ -1, /* Unassigned */ /* Keyboard Alternate Erase 99 */ -1, /* Unassigned */ /* Keyboard SysReq/Attention 9A */ -1, /* Unassigned */ /* Keyboard Cancel 9B */ -1, /* Unassigned */ /* Keyboard Clear 9C */ -1, /* Unassigned */ /* Keyboard Prior 9D */ -1, /* Unassigned */ /* Keyboard Return 9E */ -1, /* Unassigned */ /* Keyboard Separator 9F */ -1, /* Unassigned */ /* Keyboard Out A0 */ -1, /* Unassigned */ /* Keyboard Oper A1 */ -1, /* Unassigned */ /* Keyboard Clear/Again A2 */ -1, /* Unassigned */ /* Keyboard CrSel/Props A3 */ -1, /* Unassigned */ /* Keyboard ExSel A4 */ -1, /* Unassigned */ /* Reserved A5 */ -1, /* Reserved */ /* Reserved A6 */ -1, /* Reserved */ /* Reserved A7 */ -1, /* Reserved */ /* Reserved A8 */ -1, /* Reserved */ /* Reserved A9 */ -1, /* Reserved */ /* Reserved AA */ -1, /* Reserved */ /* Reserved AB */ -1, /* Reserved */ /* Reserved AC */ -1, /* Reserved */ /* Reserved AD */ -1, /* Reserved */ /* Reserved AE */ -1, /* Reserved */ /* Reserved AF */ -1, /* Reserved */ /* Reserved B0 */ -1, /* Reserved */ /* Reserved B1 */ -1, /* Reserved */ /* Reserved B2 */ -1, /* Reserved */ /* Reserved B3 */ -1, /* Reserved */ /* Reserved B4 */ -1, /* Reserved */ /* Reserved B5 */ -1, /* Reserved */ /* Reserved B6 */ -1, /* Reserved */ /* Reserved B7 */ -1, /* Reserved */ /* Reserved B8 */ -1, /* Reserved */ /* Reserved B9 */ -1, /* Reserved */ /* Reserved BA */ -1, /* Reserved */ /* Reserved BB */ -1, /* Reserved */ /* Reserved BC */ -1, /* Reserved */ /* Reserved BD */ -1, /* Reserved */ /* Reserved BE */ -1, /* Reserved */ /* Reserved BF */ -1, /* Reserved */ /* Reserved C0 */ -1, /* Reserved */ /* Reserved C1 */ -1, /* Reserved */ /* Reserved C2 */ -1, /* Reserved */ /* Reserved C3 */ -1, /* Reserved */ /* Reserved C4 */ -1, /* Reserved */ /* Reserved C5 */ -1, /* Reserved */ /* Reserved C6 */ -1, /* Reserved */ /* Reserved C7 */ -1, /* Reserved */ /* Reserved C8 */ -1, /* Reserved */ /* Reserved C9 */ -1, /* Reserved */ /* Reserved CA */ -1, /* Reserved */ /* Reserved CB */ -1, /* Reserved */ /* Reserved CC */ -1, /* Reserved */ /* Reserved CD */ -1, /* Reserved */ /* Reserved CE */ -1, /* Reserved */ /* Reserved CF */ -1, /* Reserved */ /* Reserved D0 */ -1, /* Reserved */ /* Reserved D1 */ -1, /* Reserved */ /* Reserved D2 */ -1, /* Reserved */ /* Reserved D3 */ -1, /* Reserved */ /* Reserved D4 */ -1, /* Reserved */ /* Reserved D5 */ -1, /* Reserved */ /* Reserved D6 */ -1, /* Reserved */ /* Reserved D7 */ -1, /* Reserved */ /* Reserved D8 */ -1, /* Reserved */ /* Reserved D9 */ -1, /* Reserved */ /* Reserved DA */ -1, /* Reserved */ /* Reserved DB */ -1, /* Reserved */ /* Reserved DC */ -1, /* Reserved */ /* Reserved DD */ -1, /* Reserved */ /* Reserved DE */ -1, /* Reserved */ /* Reserved DF */ -1, /* Reserved */ /* Left Control E0 */ 0x1D, /* 9D */ /* Left Shift E1 */ 0x2A, /* AA */ /* Left Alt E2 */ 0x38, /* B8 */ /* Left GUI E3 */ E0PREFIX|0x5B, /* E0 DB */ /* Right Control E4 */ E0PREFIX|0x1D, /* E0 9D */ /* Right Shift E5 */ 0x36, /* B6 */ /* Right Alt E6 */ E0PREFIX|0x38, /* E0 B8 */ /* Right GUI E7 */ E0PREFIX|0x5C /* E0 DC */ }; -#define xsize ((int32_t)(sizeof(x)/sizeof(x[0]))) +#define xsize (int32_t)nitems(x) /* * Get a max HID keycode (aligned) */ int32_t kbd_maxkey(void) { return (xsize); } /* * Process keys */ int32_t kbd_process_keys(bthid_session_p s) { bitstr_t diff[bitstr_size(xsize)]; int32_t f1, f2, i; assert(s != NULL); assert(s->srv != NULL); /* Check if the new keys have been pressed */ bit_ffs(s->keys1, xsize, &f1); /* Check if old keys still pressed */ bit_ffs(s->keys2, xsize, &f2); if (f1 == -1) { /* no new key pressed */ if (f2 != -1) { /* release old keys */ kbd_write(s->keys2, f2, 0, s->vkbd); uinput_kbd_write(s->keys2, f2, 0, s->ukbd); memset(s->keys2, 0, bitstr_size(xsize)); } return (0); } if (f2 == -1) { /* no old keys, but new keys pressed */ assert(f1 != -1); memcpy(s->keys2, s->keys1, bitstr_size(xsize)); kbd_write(s->keys1, f1, 1, s->vkbd); uinput_kbd_write(s->keys1, f1, 1, s->ukbd); memset(s->keys1, 0, bitstr_size(xsize)); return (0); } /* new keys got pressed, old keys got released */ memset(diff, 0, bitstr_size(xsize)); for (i = f2; i < xsize; i ++) { if (bit_test(s->keys2, i)) { if (!bit_test(s->keys1, i)) { bit_clear(s->keys2, i); bit_set(diff, i); } } } for (i = f1; i < xsize; i++) { if (bit_test(s->keys1, i)) { if (!bit_test(s->keys2, i)) bit_set(s->keys2, i); else bit_clear(s->keys1, i); } } bit_ffs(diff, xsize, &f2); if (f2 > 0) { kbd_write(diff, f2, 0, s->vkbd); uinput_kbd_write(diff, f2, 0, s->ukbd); } bit_ffs(s->keys1, xsize, &f1); if (f1 > 0) { kbd_write(s->keys1, f1, 1, s->vkbd); uinput_kbd_write(s->keys1, f1, 1, s->ukbd); memset(s->keys1, 0, bitstr_size(xsize)); } return (0); } /* * Translate given keymap and write keyscodes */ void uinput_kbd_write(bitstr_t *m, int32_t fb, int32_t make, int32_t fd) { int32_t i; if (fd >= 0) { for (i = fb; i < xsize; i++) { if (bit_test(m, i)) uinput_rep_key(fd, i, make); } } } /* * Translate given keymap and write keyscodes */ static void kbd_write(bitstr_t *m, int32_t fb, int32_t make, int32_t fd) { int32_t i, *b, *eob, n, buf[64]; b = buf; - eob = b + sizeof(buf)/sizeof(buf[0]); + eob = b + nitems(buf); i = fb; while (i < xsize) { if (bit_test(m, i)) { n = kbd_xlate(i, make, b, eob); if (n == -1) { write(fd, buf, (b - buf) * sizeof(buf[0])); b = buf; continue; } b += n; } i ++; } if (b != buf) write(fd, buf, (b - buf) * sizeof(buf[0])); } /* * Translate HID code into PS/2 code and put codes into buffer b. * Returns the number of codes put in b. Return -1 if buffer has not * enough space. */ #undef PUT #define PUT(c, n, b, eob) \ do { \ if ((b) >= (eob)) \ return (-1); \ *(b) = (c); \ (b) ++; \ (n) ++; \ } while (0) static int32_t kbd_xlate(int32_t code, int32_t make, int32_t *b, int32_t const *eob) { int32_t c, n; n = 0; if (code >= xsize) return (0); /* HID code is not in the table */ /* Handle special case - Pause/Break */ if (code == 0x48) { if (!make) return (0); /* No break code */ #if 0 XXX FIXME if (ctrl_is_pressed) { /* Break (Ctrl-Pause) */ PUT(0xe0, n, b, eob); PUT(0x46, n, b, eob); PUT(0xe0, n, b, eob); PUT(0xc6, n, b, eob); } else { /* Pause */ PUT(0xe1, n, b, eob); PUT(0x1d, n, b, eob); PUT(0x45, n, b, eob); PUT(0xe1, n, b, eob); PUT(0x9d, n, b, eob); PUT(0xc5, n, b, eob); } #endif return (n); } if ((c = x[code]) == -1) return (0); /* HID code translation is not defined */ if (make) { if (c & E0PREFIX) PUT(0xe0, n, b, eob); PUT((c & CODEMASK), n, b, eob); } else if (!(c & NOBREAK)) { if (c & E0PREFIX) PUT(0xe0, n, b, eob); PUT((0x80|(c & CODEMASK)), n, b, eob); } return (n); } /* * Process status change from vkbd(4) */ int32_t kbd_status_changed(bthid_session_p s, uint8_t *data, int32_t len) { vkbd_status_t st; uint8_t found, report_id; hid_device_p hid_device; hid_data_t d; hid_item_t h; uint8_t leds_mask = 0; assert(s != NULL); assert(len == sizeof(vkbd_status_t)); memcpy(&st, data, sizeof(st)); found = 0; report_id = NO_REPORT_ID; hid_device = get_hid_device(&s->bdaddr); assert(hid_device != NULL); data[0] = 0xa2; /* DATA output (HID output report) */ data[1] = 0x00; data[2] = 0x00; for (d = hid_start_parse(hid_device->desc, 1 << hid_output, -1); hid_get_item(d, &h) > 0; ) { if (HID_PAGE(h.usage) == HUP_LEDS) { found++; if (report_id == NO_REPORT_ID) report_id = h.report_ID; else if (h.report_ID != report_id) syslog(LOG_WARNING, "Output HID report IDs " \ "for %s do not match: %d vs. %d. " \ "Please report", bt_ntoa(&s->bdaddr, NULL), h.report_ID, report_id); switch(HID_USAGE(h.usage)) { case 0x01: /* Num Lock LED */ if (st.leds & LED_NUM) hid_set_data(&data[1], &h, 1); leds_mask |= LED_NUM; break; case 0x02: /* Caps Lock LED */ if (st.leds & LED_CAP) hid_set_data(&data[1], &h, 1); leds_mask |= LED_CAP; break; case 0x03: /* Scroll Lock LED */ if (st.leds & LED_SCR) hid_set_data(&data[1], &h, 1); leds_mask |= LED_SCR; break; /* XXX add other LEDs ? */ } } } hid_end_parse(d); if (report_id != NO_REPORT_ID) { data[2] = data[1]; data[1] = report_id; } if (found) write(s->intr, data, (report_id != NO_REPORT_ID) ? 3 : 2); if (found && s->srv->uinput && hid_device->keyboard) uinput_rep_leds(s->ukbd, st.leds, leds_mask); return (0); } diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.c b/usr.sbin/bluetooth/hccontrol/hccontrol.c index c1ce016e6246..bd63c9aff6ec 100644 --- a/usr.sbin/bluetooth/hccontrol/hccontrol.c +++ b/usr.sbin/bluetooth/hccontrol/hccontrol.c @@ -1,333 +1,334 @@ /*- * hccontrol.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2001-2002 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: hccontrol.c,v 1.5 2003/09/05 00:38:24 max Exp $ */ #define L2CAP_SOCKET_CHECKED #include #include +#include #include #include #include #include #include #include #include #include #include #include "hccontrol.h" /* Prototypes */ static int do_hci_command (char const *, int, char **); static struct hci_command * find_hci_command (char const *, struct hci_command *); static int find_hci_nodes (struct nodeinfo **); static void print_hci_command (struct hci_command *); static void usage (void); /* Globals */ int verbose = 0; int timeout; int numeric_bdaddr = 0; /* Main */ int main(int argc, char *argv[]) { char *node = NULL; int n; /* Process command line arguments */ while ((n = getopt(argc, argv, "n:Nvh")) != -1) { switch (n) { case 'n': node = optarg; break; case 'N': numeric_bdaddr = 1; break; case 'v': verbose = 1; break; case 'h': default: usage(); } } argc -= optind; argv += optind; if (*argv == NULL) usage(); n = do_hci_command(node, argc, argv); return (n); } /* main */ /* Create socket and bind it */ static int socket_open(char const *node) { struct sockaddr_hci addr; struct ng_btsocket_hci_raw_filter filter; int s, mib[4], num; size_t size; struct nodeinfo *nodes; char *lnode = NULL; num = find_hci_nodes(&nodes); if (num == 0) errx(7, "Could not find HCI nodes"); if (node == NULL) { node = lnode = strdup(nodes[0].name); if (num > 1) fprintf(stdout, "Using HCI node: %s\n", node); } free(nodes); s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI); if (s < 0) err(1, "Could not create socket"); memset(&addr, 0, sizeof(addr)); addr.hci_len = sizeof(addr); addr.hci_family = AF_BLUETOOTH; strncpy(addr.hci_node, node, sizeof(addr.hci_node)); if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) err(2, "Could not bind socket, node=%s", node); if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) err(3, "Could not connect socket, node=%s", node); free(lnode); memset(&filter, 0, sizeof(filter)); bit_set(filter.event_mask, NG_HCI_EVENT_COMMAND_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_COMMAND_STATUS - 1); bit_set(filter.event_mask, NG_HCI_EVENT_INQUIRY_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_INQUIRY_RESULT - 1); bit_set(filter.event_mask, NG_HCI_EVENT_CON_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_DISCON_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_REMOTE_NAME_REQ_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_READ_REMOTE_FEATURES_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_READ_REMOTE_VER_INFO_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_RETURN_LINK_KEYS - 1); bit_set(filter.event_mask, NG_HCI_EVENT_READ_CLOCK_OFFSET_COMPL - 1); bit_set(filter.event_mask, NG_HCI_EVENT_CON_PKT_TYPE_CHANGED - 1); bit_set(filter.event_mask, NG_HCI_EVENT_ROLE_CHANGE - 1); bit_set(filter.event_mask, NG_HCI_EVENT_LE -1); if (setsockopt(s, SOL_HCI_RAW, SO_HCI_RAW_FILTER, (void * const) &filter, sizeof(filter)) < 0) err(4, "Could not setsockopt()"); - size = (sizeof(mib)/sizeof(mib[0])); + size = nitems(mib); if (sysctlnametomib("net.bluetooth.hci.command_timeout",mib,&size) < 0) err(5, "Could not sysctlnametomib()"); - if (sysctl(mib, sizeof(mib)/sizeof(mib[0]), + if (sysctl(mib, nitems(mib), (void *) &timeout, &size, NULL, 0) < 0) err(6, "Could not sysctl()"); timeout ++; return (s); } /* socket_open */ /* Execute commands */ static int do_hci_command(char const *node, int argc, char **argv) { char *cmd = argv[0]; struct hci_command *c = NULL; int s, e, help; help = 0; if (strcasecmp(cmd, "help") == 0) { argc --; argv ++; if (argc <= 0) { fprintf(stdout, "Supported commands:\n"); print_hci_command(link_control_commands); print_hci_command(link_policy_commands); print_hci_command(host_controller_baseband_commands); print_hci_command(info_commands); print_hci_command(status_commands); print_hci_command(le_commands); print_hci_command(node_commands); fprintf(stdout, "\nFor more information use " \ "'help command'\n"); return (OK); } help = 1; cmd = argv[0]; } c = find_hci_command(cmd, link_control_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, link_policy_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, host_controller_baseband_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, info_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, status_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, le_commands); if (c != NULL) goto execute; c = find_hci_command(cmd, node_commands); if (c == NULL) { fprintf(stdout, "Unknown command: \"%s\"\n", cmd); return (ERROR); } execute: if (!help) { s = socket_open(node); e = (c->handler)(s, -- argc, ++ argv); close(s); } else e = USAGE; switch (e) { case OK: case FAILED: break; case ERROR: fprintf(stdout, "Could not execute command \"%s\". %s\n", cmd, strerror(errno)); break; case USAGE: fprintf(stdout, "Usage: %s\n%s\n", c->command, c->description); break; default: assert(0); break; } return (e); } /* do_hci_command */ /* Try to find command in specified category */ static struct hci_command * find_hci_command(char const *command, struct hci_command *category) { struct hci_command *c = NULL; for (c = category; c->command != NULL; c++) { char *c_end = strchr(c->command, ' '); if (c_end != NULL) { int len = c_end - c->command; if (strncasecmp(command, c->command, len) == 0) return (c); } else if (strcasecmp(command, c->command) == 0) return (c); } return (NULL); } /* find_hci_command */ /* Find all HCI nodes */ static int find_hci_nodes(struct nodeinfo** nodes) { struct ng_btsocket_hci_raw_node_list_names r; struct sockaddr_hci addr; int s; const char * node = "ubt0hci"; r.num_names = MAX_NODE_NUM; r.names = (struct nodeinfo*)calloc(MAX_NODE_NUM, sizeof(struct nodeinfo)); if (r.names == NULL) err(8, "Could not allocate memory"); s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI); if (s < 0) err(9, "Could not create socket"); memset(&addr, 0, sizeof(addr)); addr.hci_len = sizeof(addr); addr.hci_family = AF_BLUETOOTH; strncpy(addr.hci_node, node, sizeof(addr.hci_node)); if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) err(10, "Could not bind socket"); if (ioctl(s, SIOC_HCI_RAW_NODE_LIST_NAMES, &r, sizeof(r)) < 0) err(11, "Could not get list of HCI nodes"); close(s); *nodes = r.names; return (r.num_names); } /* find_hci_nodes */ /* Print commands in specified category */ static void print_hci_command(struct hci_command *category) { struct hci_command *c = NULL; for (c = category; c->command != NULL; c++) fprintf(stdout, "\t%s\n", c->command); } /* print_hci_command */ /* Usage */ static void usage(void) { fprintf(stdout, "Usage: hccontrol [-hN] [-n HCI_node_name] cmd [p1] [..]\n"); exit(255); } /* usage */ diff --git a/usr.sbin/bluetooth/hccontrol/node.c b/usr.sbin/bluetooth/hccontrol/node.c index b100900f527d..61b60ba95db5 100644 --- a/usr.sbin/bluetooth/hccontrol/node.c +++ b/usr.sbin/bluetooth/hccontrol/node.c @@ -1,619 +1,620 @@ /*- * node.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2001-2002 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: node.c,v 1.6 2003/07/22 21:14:02 max Exp $ */ #include +#include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include #include #include #include #include "hccontrol.h" /* Send Read_Node_State command to the node */ static int hci_read_node_state(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_state r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STATE, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "State: %#x\n", r.state); return (OK); } /* hci_read_node_state */ /* Send Intitialize command to the node */ static int hci_node_initialize(int s, int argc, char **argv) { if (ioctl(s, SIOC_HCI_RAW_NODE_INIT) < 0) return (ERROR); return (OK); } /* hci_node_initialize */ /* Send Read_Debug_Level command to the node */ static int hci_read_debug_level(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_debug r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_DEBUG, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Debug level: %d\n", r.debug); return (OK); } /* hci_read_debug_level */ /* Send Write_Debug_Level command to the node */ static int hci_write_debug_level(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_debug r; memset(&r, 0, sizeof(r)); switch (argc) { case 1: r.debug = atoi(argv[0]); break; default: return (USAGE); } if (ioctl(s, SIOC_HCI_RAW_NODE_SET_DEBUG, &r, sizeof(r)) < 0) return (ERROR); return (OK); } /* hci_write_debug_level */ /* Send Read_Node_Buffer_Size command to the node */ static int hci_read_node_buffer_size(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_buffer r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BUFFER, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Number of free command buffers: %d\n", r.buffer.cmd_free); fprintf(stdout, "Max. ACL packet size: %d\n", r.buffer.acl_size); fprintf(stdout, "Numbef of free ACL buffers: %d\n", r.buffer.acl_free); fprintf(stdout, "Total number of ACL buffers: %d\n", r.buffer.acl_pkts); fprintf(stdout, "Max. SCO packet size: %d\n", r.buffer.sco_size); fprintf(stdout, "Numbef of free SCO buffers: %d\n", r.buffer.sco_free); fprintf(stdout, "Total number of SCO buffers: %d\n", r.buffer.sco_pkts); return (OK); } /* hci_read_node_buffer_size */ /* Send Read_Node_BD_ADDR command to the node */ static int hci_read_node_bd_addr(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_bdaddr r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BDADDR, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "BD_ADDR: %s\n", bt_ntoa(&r.bdaddr, NULL)); return (OK); } /* hci_read_node_bd_addr */ /* Send Read_Node_Features command to the node */ static int hci_read_node_features(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_features r; int n; char buffer[2048]; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_FEATURES, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Features: "); - for (n = 0; n < sizeof(r.features)/sizeof(r.features[0]); n++) + for (n = 0; n < nitems(r.features); n++) fprintf(stdout, "%#02x ", r.features[n]); fprintf(stdout, "\n%s\n", hci_features2str(r.features, buffer, sizeof(buffer))); return (OK); } /* hci_read_node_features */ /* Send Read_Node_Stat command to the node */ static int hci_read_node_stat(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_stat r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STAT, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Commands sent: %d\n", r.stat.cmd_sent); fprintf(stdout, "Events received: %d\n", r.stat.evnt_recv); fprintf(stdout, "ACL packets received: %d\n", r.stat.acl_recv); fprintf(stdout, "ACL packets sent: %d\n", r.stat.acl_sent); fprintf(stdout, "SCO packets received: %d\n", r.stat.sco_recv); fprintf(stdout, "SCO packets sent: %d\n", r.stat.sco_sent); fprintf(stdout, "Bytes received: %d\n", r.stat.bytes_recv); fprintf(stdout, "Bytes sent: %d\n", r.stat.bytes_sent); return (OK); } /* hci_read_node_stat */ /* Send Reset_Node_Stat command to the node */ static int hci_reset_node_stat(int s, int argc, char **argv) { if (ioctl(s, SIOC_HCI_RAW_NODE_RESET_STAT) < 0) return (ERROR); return (OK); } /* hci_reset_node_stat */ /* Send Flush_Neighbor_Cache command to the node */ static int hci_flush_neighbor_cache(int s, int argc, char **argv) { if (ioctl(s, SIOC_HCI_RAW_NODE_FLUSH_NEIGHBOR_CACHE) < 0) return (ERROR); return (OK); } /* hci_flush_neighbor_cache */ /* Send Read_Neighbor_Cache command to the node */ static int hci_read_neighbor_cache(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_neighbor_cache r; int n, error = OK; const char *addrtype2str[] = {"B", "P", "R", "E"}; memset(&r, 0, sizeof(r)); r.num_entries = NG_HCI_MAX_NEIGHBOR_NUM; r.entries = calloc(NG_HCI_MAX_NEIGHBOR_NUM, sizeof(ng_hci_node_neighbor_cache_entry_ep)); if (r.entries == NULL) { errno = ENOMEM; return (ERROR); } if (ioctl(s, SIOC_HCI_RAW_NODE_GET_NEIGHBOR_CACHE, &r, sizeof(r)) < 0) { error = ERROR; goto out; } fprintf(stdout, "T " \ "BD_ADDR " \ "Features " \ "Clock offset " \ "Page scan " \ "Rep. scan\n"); for (n = 0; n < r.num_entries; n++) { uint8_t addrtype = r.entries[n].addrtype; - if(addrtype >= sizeof(addrtype2str)/sizeof(addrtype2str[0])) - addrtype = sizeof(addrtype2str)/sizeof(addrtype2str[0]) - 1; + if(addrtype >= nitems(addrtype2str)) + addrtype = nitems(addrtype2str) - 1; fprintf(stdout, "%1s %-17.17s " \ "%02x %02x %02x %02x %02x %02x %02x %02x " \ "%#12x " \ "%#9x " \ "%#9x\n", addrtype2str[addrtype], hci_bdaddr2str(&r.entries[n].bdaddr), r.entries[n].features[0], r.entries[n].features[1], r.entries[n].features[2], r.entries[n].features[3], r.entries[n].features[4], r.entries[n].features[5], r.entries[n].features[6], r.entries[n].features[7], r.entries[n].clock_offset, r.entries[n].page_scan_mode, r.entries[n].page_scan_rep_mode); print_adv_data(r.entries[n].extinq_size, r.entries[n].extinq_data); fprintf(stdout,"\n"); } out: free(r.entries); return (error); } /* hci_read_neightbor_cache */ /* Send Read_Connection_List command to the node */ static int hci_read_connection_list(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_con_list r; int n, error = OK; memset(&r, 0, sizeof(r)); r.num_connections = NG_HCI_MAX_CON_NUM; r.connections = calloc(NG_HCI_MAX_CON_NUM, sizeof(ng_hci_node_con_ep)); if (r.connections == NULL) { errno = ENOMEM; return (ERROR); } if (ioctl(s, SIOC_HCI_RAW_NODE_GET_CON_LIST, &r, sizeof(r)) < 0) { error = ERROR; goto out; } fprintf(stdout, "Remote BD_ADDR " \ "Handle " \ "Type " \ "Mode " \ "Role " \ "Encrypt " \ "Pending " \ "Queue " \ "State\n"); for (n = 0; n < r.num_connections; n++) { fprintf(stdout, "%-17.17s " \ "%6d " \ "%4.4s " \ "%4d " \ "%4.4s " \ "%7.7s " \ "%7d " \ "%5d " \ "%s\n", hci_bdaddr2str(&r.connections[n].bdaddr), r.connections[n].con_handle, (r.connections[n].link_type == NG_HCI_LINK_ACL)? "ACL" : "SCO", r.connections[n].mode, (r.connections[n].role == NG_HCI_ROLE_MASTER)? "MAST" : "SLAV", hci_encrypt2str(r.connections[n].encryption_mode, 1), r.connections[n].pending, r.connections[n].queue_len, hci_con_state2str(r.connections[n].state)); } out: free(r.connections); return (error); } /* hci_read_connection_list */ /* Send Read_Node_Link_Policy_Settings_Mask command to the node */ int hci_read_node_link_policy_settings_mask(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_link_policy_mask r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_LINK_POLICY_MASK, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Link Policy Settings mask: %#04x\n", r.policy_mask); return (OK); } /* hci_read_node_link_policy_settings_mask */ /* Send Write_Node_Link_Policy_Settings_Mask command to the node */ int hci_write_node_link_policy_settings_mask(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_link_policy_mask r; int m; memset(&r, 0, sizeof(r)); switch (argc) { case 1: if (sscanf(argv[0], "%x", &m) != 1) return (USAGE); r.policy_mask = (m & 0xffff); break; default: return (USAGE); } if (ioctl(s, SIOC_HCI_RAW_NODE_SET_LINK_POLICY_MASK, &r, sizeof(r)) < 0) return (ERROR); return (OK); } /* hci_write_node_link_policy_settings_mask */ /* Send Read_Node_Packet_Mask command to the node */ int hci_read_node_packet_mask(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_packet_mask r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_PACKET_MASK, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Packet mask: %#04x\n", r.packet_mask); return (OK); } /* hci_read_node_packet_mask */ /* Send Write_Node_Packet_Mask command to the node */ int hci_write_node_packet_mask(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_packet_mask r; int m; memset(&r, 0, sizeof(r)); switch (argc) { case 1: if (sscanf(argv[0], "%x", &m) != 1) return (USAGE); r.packet_mask = (m & 0xffff); break; default: return (USAGE); } if (ioctl(s, SIOC_HCI_RAW_NODE_SET_PACKET_MASK, &r, sizeof(r)) < 0) return (ERROR); return (OK); } /* hci_write_node_packet_mask */ /* Send Read_Node_Role_Switch command to the node */ int hci_read_node_role_switch(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_role_switch r; memset(&r, 0, sizeof(r)); if (ioctl(s, SIOC_HCI_RAW_NODE_GET_ROLE_SWITCH, &r, sizeof(r)) < 0) return (ERROR); fprintf(stdout, "Role switch: %d\n", r.role_switch); return (OK); } /* hci_read_node_role_switch */ /* Send Write_Node_Role_Switch command to the node */ int hci_write_node_role_switch(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_role_switch r; int m; memset(&r, 0, sizeof(r)); switch (argc) { case 1: if (sscanf(argv[0], "%d", &m) != 1) return (USAGE); r.role_switch = m? 1 : 0; break; default: return (USAGE); } if (ioctl(s, SIOC_HCI_RAW_NODE_SET_ROLE_SWITCH, &r, sizeof(r)) < 0) return (ERROR); return (OK); } /* hci_write_node_role_switch */ /* Send Read_Node_List command to the node */ int hci_read_node_list(int s, int argc, char **argv) { struct ng_btsocket_hci_raw_node_list_names r; int i; r.num_names = MAX_NODE_NUM; r.names = (struct nodeinfo*)calloc(MAX_NODE_NUM, sizeof(struct nodeinfo)); if (r.names == NULL) return (ERROR); if (ioctl(s, SIOC_HCI_RAW_NODE_LIST_NAMES, &r, sizeof(r)) < 0) { free(r.names); return (ERROR); } fprintf(stdout, "Name ID Num hooks\n"); for (i = 0; i < r.num_names; ++i) fprintf(stdout, "%-15s %08x %9d\n", r.names[i].name, r.names[i].id, r.names[i].hooks); free(r.names); return (OK); } /* hci_read_node_list */ struct hci_command node_commands[] = { { "read_node_state", "Get the HCI node state", &hci_read_node_state }, { "initialize", "Initialize the HCI node", &hci_node_initialize }, { "read_debug_level", "Read the HCI node debug level", &hci_read_debug_level }, { "write_debug_level ", "Write the HCI node debug level", &hci_write_debug_level }, { "read_node_buffer_size", "Read the HCI node buffer information. This will return current state of the\n"\ "HCI buffer for the HCI node", &hci_read_node_buffer_size }, { "read_node_bd_addr", "Read the HCI node BD_ADDR. Returns device BD_ADDR as cached by the HCI node", &hci_read_node_bd_addr }, { "read_node_features", "Read the HCI node features. This will return list of supported features as\n" \ "cached by the HCI node", &hci_read_node_features }, { "read_node_stat", "Read packets and bytes counters for the HCI node", &hci_read_node_stat }, { "reset_node_stat", "Reset packets and bytes counters for the HCI node", &hci_reset_node_stat }, { "flush_neighbor_cache", "Flush content of the HCI node neighbor cache", &hci_flush_neighbor_cache }, { "read_neighbor_cache", "Read content of the HCI node neighbor cache", &hci_read_neighbor_cache }, { "read_connection_list", "Read the baseband connection descriptors list for the HCI node", &hci_read_connection_list }, { "read_node_link_policy_settings_mask", "Read the value of the Link Policy Settinngs mask for the HCI node", &hci_read_node_link_policy_settings_mask }, { "write_node_link_policy_settings_mask ", "Write the value of the Link Policy Settings mask for the HCI node. By default\n" \ "all supported Link Policy modes (as reported by the local device features) are\n"\ "enabled. The particular Link Policy mode is enabled if local device supports\n"\ "it and correspinding bit in the mask was set\n\n" \ "\t - xxxx; Link Policy mask\n" \ "\t\t0x0000 - Disable All LM Modes\n" \ "\t\t0x0001 - Enable Master Slave Switch\n" \ "\t\t0x0002 - Enable Hold Mode\n" \ "\t\t0x0004 - Enable Sniff Mode\n" \ "\t\t0x0008 - Enable Park Mode\n", &hci_write_node_link_policy_settings_mask }, { "read_node_packet_mask", "Read the value of the Packet mask for the HCI node", &hci_read_node_packet_mask }, { "write_node_packet_mask ", "Write the value of the Packet mask for the HCI node. By default all supported\n" \ "packet types (as reported by the local device features) are enabled. The\n" \ "particular packet type is enabled if local device supports it and corresponding\n" \ "bit in the mask was set\n\n" \ "\t - xxxx; packet type mask\n" \ "" \ "\t\tACL packets\n" \ "\t\t-----------\n" \ "\t\t0x0008 DM1\n" \ "\t\t0x0010 DH1\n" \ "\t\t0x0400 DM3\n" \ "\t\t0x0800 DH3\n" \ "\t\t0x4000 DM5\n" \ "\t\t0x8000 DH5\n" \ "\n" \ "\t\tSCO packets\n" \ "\t\t-----------\n" \ "\t\t0x0020 HV1\n" \ "\t\t0x0040 HV2\n" \ "\t\t0x0080 HV3\n", &hci_write_node_packet_mask }, { "read_node_role_switch", "Read the value of the Role Switch parameter for the HCI node", &hci_read_node_role_switch }, { "write_node_role_switch {0|1}", "Write the value of the Role Switch parameter for the HCI node. By default,\n" \ "if Role Switch is supported, local device will try to perform Role Switch\n" \ "and become Master on incoming connection. Some devices do not support Role\n" \ "Switch and thus incoming connections from such devices will fail. Setting\n" \ "this parameter to zero will prevent Role Switch and thus accepting device\n" \ "will remain Slave", &hci_write_node_role_switch }, { "read_node_list", "Get a list of HCI nodes, their Netgraph IDs and connected hooks.", &hci_read_node_list }, { NULL, }}; diff --git a/usr.sbin/bluetooth/sdpcontrol/search.c b/usr.sbin/bluetooth/sdpcontrol/search.c index 94c34828a949..2a4b2468c7f7 100644 --- a/usr.sbin/bluetooth/sdpcontrol/search.c +++ b/usr.sbin/bluetooth/sdpcontrol/search.c @@ -1,749 +1,750 @@ /*- * search.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2001-2003 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: search.c,v 1.2 2003/09/08 17:35:15 max Exp $ */ +#include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include #include "sdpcontrol.h" /* List of the attributes we are looking for */ static uint32_t attrs[] = { SDP_ATTR_RANGE( SDP_ATTR_SERVICE_RECORD_HANDLE, SDP_ATTR_SERVICE_RECORD_HANDLE), SDP_ATTR_RANGE( SDP_ATTR_SERVICE_CLASS_ID_LIST, SDP_ATTR_SERVICE_CLASS_ID_LIST), SDP_ATTR_RANGE( SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST, SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST), SDP_ATTR_RANGE( SDP_ATTR_BLUETOOTH_PROFILE_DESCRIPTOR_LIST, SDP_ATTR_BLUETOOTH_PROFILE_DESCRIPTOR_LIST) }; -#define attrs_len (sizeof(attrs)/sizeof(attrs[0])) +#define attrs_len nitems(attrs) /* Buffer for the attributes */ #define NRECS 25 /* request this much records from the SDP server */ #define BSIZE 256 /* one attribute buffer size */ static uint8_t buffer[NRECS * attrs_len][BSIZE]; /* SDP attributes */ static sdp_attr_t values[NRECS * attrs_len]; -#define values_len (sizeof(values)/sizeof(values[0])) +#define values_len nitems(values) /* * Print Service Class ID List * * The ServiceClassIDList attribute consists of a data element sequence in * which each data element is a UUID representing the service classes that * a given service record conforms to. The UUIDs are listed in order from * the most specific class to the most general class. The ServiceClassIDList * must contain at least one service class UUID. */ static void print_service_class_id_list(uint8_t const *start, uint8_t const *end) { uint32_t type, len, value; if (end - start < 2) { fprintf(stderr, "Invalid Service Class ID List. " \ "Too short, len=%zd\n", end - start); return; } SDP_GET8(type, start); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, start); break; case SDP_DATA_SEQ16: SDP_GET16(len, start); break; case SDP_DATA_SEQ32: SDP_GET32(len, start); break; default: fprintf(stderr, "Invalid Service Class ID List. " \ "Not a sequence, type=%#x\n", type); return; /* NOT REACHED */ } if (len > (end - start)) { fprintf(stderr, "Invalid Service Class ID List. " \ "Too long len=%d\n", len); return; } while (start < end) { SDP_GET8(type, start); switch (type) { case SDP_DATA_UUID16: SDP_GET16(value, start); fprintf(stdout, "\t%s (%#4.4x)\n", sdp_uuid2desc(value), value); break; case SDP_DATA_UUID32: SDP_GET32(value, start); fprintf(stdout, "\t%#8.8x\n", value); break; case SDP_DATA_UUID128: { int128_t uuid; SDP_GET_UUID128(&uuid, start); fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n", ntohl(*(uint32_t *)&uuid.b[0]), ntohs(*(uint16_t *)&uuid.b[4]), ntohs(*(uint16_t *)&uuid.b[6]), ntohs(*(uint16_t *)&uuid.b[8]), ntohs(*(uint16_t *)&uuid.b[10]), ntohl(*(uint32_t *)&uuid.b[12])); } break; default: fprintf(stderr, "Invalid Service Class ID List. " \ "Not a UUID, type=%#x\n", type); return; /* NOT REACHED */ } } } /* print_service_class_id_list */ /* * Print Protocol Descriptor List * * If the ProtocolDescriptorList describes a single stack, it takes the form * of a data element sequence in which each element of the sequence is a * protocol descriptor. Each protocol descriptor is, in turn, a data element * sequence whose first element is a UUID identifying the protocol and whose * successive elements are protocol-specific parameters. The protocol * descriptors are listed in order from the lowest layer protocol to the * highest layer protocol used to gain access to the service. If it is possible * for more than one kind of protocol stack to be used to gain access to the * service, the ProtocolDescriptorList takes the form of a data element * alternative where each member is a data element sequence as described above. */ static void print_protocol_descriptor(uint8_t const *start, uint8_t const *end) { union { uint8_t uint8; uint16_t uint16; uint32_t uint32; uint64_t uint64; int128_t int128; } value; uint32_t type, len, param; /* Get Protocol UUID */ SDP_GET8(type, start); switch (type) { case SDP_DATA_UUID16: SDP_GET16(value.uint16, start); fprintf(stdout, "\t%s (%#4.4x)\n", sdp_uuid2desc(value.uint16), value.uint16); break; case SDP_DATA_UUID32: SDP_GET32(value.uint32, start); fprintf(stdout, "\t%#8.8x\n", value.uint32); break; case SDP_DATA_UUID128: SDP_GET_UUID128(&value.int128, start); fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n", ntohl(*(uint32_t *)&value.int128.b[0]), ntohs(*(uint16_t *)&value.int128.b[4]), ntohs(*(uint16_t *)&value.int128.b[6]), ntohs(*(uint16_t *)&value.int128.b[8]), ntohs(*(uint16_t *)&value.int128.b[10]), ntohl(*(uint32_t *)&value.int128.b[12])); break; default: fprintf(stderr, "Invalid Protocol Descriptor. " \ "Not a UUID, type=%#x\n", type); return; /* NOT REACHED */ } /* Protocol specific parameters */ for (param = 1; start < end; param ++) { fprintf(stdout, "\t\tProtocol specific parameter #%d: ", param); SDP_GET8(type, start); switch (type) { case SDP_DATA_NIL: fprintf(stdout, "nil\n"); break; case SDP_DATA_UINT8: case SDP_DATA_INT8: case SDP_DATA_BOOL: SDP_GET8(value.uint8, start); fprintf(stdout, "u/int8/bool %u\n", value.uint8); break; case SDP_DATA_UINT16: case SDP_DATA_INT16: case SDP_DATA_UUID16: SDP_GET16(value.uint16, start); fprintf(stdout, "u/int/uuid16 %u\n", value.uint16); break; case SDP_DATA_UINT32: case SDP_DATA_INT32: case SDP_DATA_UUID32: SDP_GET32(value.uint32, start); fprintf(stdout, "u/int/uuid32 %u\n", value.uint32); break; case SDP_DATA_UINT64: case SDP_DATA_INT64: SDP_GET64(value.uint64, start); fprintf(stdout, "u/int64 %ju\n", value.uint64); break; case SDP_DATA_UINT128: case SDP_DATA_INT128: SDP_GET128(&value.int128, start); fprintf(stdout, "u/int128 %#8.8x%8.8x%8.8x%8.8x\n", *(uint32_t *)&value.int128.b[0], *(uint32_t *)&value.int128.b[4], *(uint32_t *)&value.int128.b[8], *(uint32_t *)&value.int128.b[12]); break; case SDP_DATA_UUID128: SDP_GET_UUID128(&value.int128, start); fprintf(stdout, "uuid128 %#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x\n", ntohl(*(uint32_t *)&value.int128.b[0]), ntohs(*(uint16_t *)&value.int128.b[4]), ntohs(*(uint16_t *)&value.int128.b[6]), ntohs(*(uint16_t *)&value.int128.b[8]), ntohs(*(uint16_t *)&value.int128.b[10]), ntohl(*(uint32_t *)&value.int128.b[12])); break; case SDP_DATA_STR8: case SDP_DATA_URL8: SDP_GET8(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%c", *start); fprintf(stdout, "\n"); break; case SDP_DATA_STR16: case SDP_DATA_URL16: SDP_GET16(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%c", *start); fprintf(stdout, "\n"); break; case SDP_DATA_STR32: case SDP_DATA_URL32: SDP_GET32(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%c", *start); fprintf(stdout, "\n"); break; case SDP_DATA_SEQ8: case SDP_DATA_ALT8: SDP_GET8(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%#2.2x ", *start); fprintf(stdout, "\n"); break; case SDP_DATA_SEQ16: case SDP_DATA_ALT16: SDP_GET16(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%#2.2x ", *start); fprintf(stdout, "\n"); break; case SDP_DATA_SEQ32: case SDP_DATA_ALT32: SDP_GET32(len, start); for (; start < end && len > 0; start ++, len --) fprintf(stdout, "%#2.2x ", *start); fprintf(stdout, "\n"); break; default: fprintf(stderr, "Invalid Protocol Descriptor. " \ "Unknown data type: %#02x\n", type); return; /* NOT REACHED */ } } } /* print_protocol_descriptor */ static void print_protocol_descriptor_list(uint8_t const *start, uint8_t const *end) { uint32_t type, len; if (end - start < 2) { fprintf(stderr, "Invalid Protocol Descriptor List. " \ "Too short, len=%zd\n", end - start); return; } SDP_GET8(type, start); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, start); break; case SDP_DATA_SEQ16: SDP_GET16(len, start); break; case SDP_DATA_SEQ32: SDP_GET32(len, start); break; default: fprintf(stderr, "Invalid Protocol Descriptor List. " \ "Not a sequence, type=%#x\n", type); return; /* NOT REACHED */ } if (len > (end - start)) { fprintf(stderr, "Invalid Protocol Descriptor List. " \ "Too long, len=%d\n", len); return; } while (start < end) { SDP_GET8(type, start); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, start); break; case SDP_DATA_SEQ16: SDP_GET16(len, start); break; case SDP_DATA_SEQ32: SDP_GET32(len, start); break; default: fprintf(stderr, "Invalid Protocol Descriptor List. " \ "Not a sequence, type=%#x\n", type); return; /* NOT REACHED */ } if (len > (end - start)) { fprintf(stderr, "Invalid Protocol Descriptor List. " \ "Too long, len=%d\n", len); return; } print_protocol_descriptor(start, start + len); start += len; } } /* print_protocol_descriptor_list */ /* * Print Bluetooth Profile Descriptor List * * The BluetoothProfileDescriptorList attribute consists of a data element * sequence in which each element is a profile descriptor that contains * information about a Bluetooth profile to which the service represented by * this service record conforms. Each profile descriptor is a data element * sequence whose first element is the UUID assigned to the profile and whose * second element is a 16-bit profile version number. Each version of a profile * is assigned a 16-bit unsigned integer profile version number, which consists * of two 8-bit fields. The higher-order 8 bits contain the major version * number field and the lower-order 8 bits contain the minor version number * field. */ static void print_bluetooth_profile_descriptor_list(uint8_t const *start, uint8_t const *end) { uint32_t type, len, value; if (end - start < 2) { fprintf(stderr, "Invalid Bluetooth Profile Descriptor List. " \ "Too short, len=%zd\n", end - start); return; } SDP_GET8(type, start); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, start); break; case SDP_DATA_SEQ16: SDP_GET16(len, start); break; case SDP_DATA_SEQ32: SDP_GET32(len, start); break; default: fprintf(stderr, "Invalid Bluetooth Profile Descriptor List. " \ "Not a sequence, type=%#x\n", type); return; /* NOT REACHED */ } if (len > (end - start)) { fprintf(stderr, "Invalid Bluetooth Profile Descriptor List. " \ "Too long, len=%d\n", len); return; } while (start < end) { SDP_GET8(type, start); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(len, start); break; case SDP_DATA_SEQ16: SDP_GET16(len, start); break; case SDP_DATA_SEQ32: SDP_GET32(len, start); break; default: fprintf(stderr, "Invalid Bluetooth Profile " \ "Descriptor List. " \ "Not a sequence, type=%#x\n", type); return; /* NOT REACHED */ } if (len > (end - start)) { fprintf(stderr, "Invalid Bluetooth Profile " \ "Descriptor List. " \ "Too long, len=%d\n", len); return; } /* Get UUID */ SDP_GET8(type, start); switch (type) { case SDP_DATA_UUID16: SDP_GET16(value, start); fprintf(stdout, "\t%s (%#4.4x) ", sdp_uuid2desc(value), value); break; case SDP_DATA_UUID32: SDP_GET32(value, start); fprintf(stdout, "\t%#8.8x ", value); break; case SDP_DATA_UUID128: { int128_t uuid; SDP_GET_UUID128(&uuid, start); fprintf(stdout, "\t%#8.8x-%4.4x-%4.4x-%4.4x-%4.4x%8.8x ", ntohl(*(uint32_t *)&uuid.b[0]), ntohs(*(uint16_t *)&uuid.b[4]), ntohs(*(uint16_t *)&uuid.b[6]), ntohs(*(uint16_t *)&uuid.b[8]), ntohs(*(uint16_t *)&uuid.b[10]), ntohl(*(uint32_t *)&uuid.b[12])); } break; default: fprintf(stderr, "Invalid Bluetooth Profile " \ "Descriptor List. " \ "Not a UUID, type=%#x\n", type); return; /* NOT REACHED */ } /* Get version */ SDP_GET8(type, start); if (type != SDP_DATA_UINT16) { fprintf(stderr, "Invalid Bluetooth Profile " \ "Descriptor List. " \ "Invalid version type=%#x\n", type); return; } SDP_GET16(value, start); fprintf(stdout, "ver. %d.%d\n", (value >> 8) & 0xff, value & 0xff); } } /* print_bluetooth_profile_descriptor_list */ /* Perform SDP search command */ static int do_sdp_search(void *xs, int argc, char **argv) { char *ep = NULL; int32_t n, type, value; uint16_t service; /* Parse command line arguments */ switch (argc) { case 1: n = strtoul(argv[0], &ep, 16); if (*ep != 0) { switch (tolower(argv[0][0])) { case 'c': /* CIP/CTP */ switch (tolower(argv[0][1])) { case 'i': service = SDP_SERVICE_CLASS_COMMON_ISDN_ACCESS; break; case 't': service = SDP_SERVICE_CLASS_CORDLESS_TELEPHONY; break; default: return (USAGE); /* NOT REACHED */ } break; case 'd': /* DialUp Networking */ service = SDP_SERVICE_CLASS_DIALUP_NETWORKING; break; case 'f': /* Fax/OBEX File Transfer */ switch (tolower(argv[0][1])) { case 'a': service = SDP_SERVICE_CLASS_FAX; break; case 't': service = SDP_SERVICE_CLASS_OBEX_FILE_TRANSFER; break; default: return (USAGE); /* NOT REACHED */ } break; case 'g': /* GN */ service = SDP_SERVICE_CLASS_GN; break; case 'h': /* Headset/HID */ switch (tolower(argv[0][1])) { case 'i': service = SDP_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE; break; case 's': service = SDP_SERVICE_CLASS_HEADSET; break; default: return (USAGE); /* NOT REACHED */ } break; case 'l': /* LAN Access Using PPP */ service = SDP_SERVICE_CLASS_LAN_ACCESS_USING_PPP; break; case 'n': /* NAP */ service = SDP_SERVICE_CLASS_NAP; break; case 'o': /* OBEX Object Push */ service = SDP_SERVICE_CLASS_OBEX_OBJECT_PUSH; break; case 's': /* Serial Port */ service = SDP_SERVICE_CLASS_SERIAL_PORT; break; default: return (USAGE); /* NOT REACHED */ } } else service = (uint16_t) n; break; default: return (USAGE); } /* Initialize attribute values array */ for (n = 0; n < values_len; n ++) { values[n].flags = SDP_ATTR_INVALID; values[n].attr = 0; values[n].vlen = BSIZE; values[n].value = buffer[n]; } /* Do SDP Service Search Attribute Request */ n = sdp_search(xs, 1, &service, attrs_len, attrs, values_len, values); if (n != 0) return (ERROR); /* Print attributes values */ for (n = 0; n < values_len; n ++) { if (values[n].flags != SDP_ATTR_OK) break; switch (values[n].attr) { case SDP_ATTR_SERVICE_RECORD_HANDLE: fprintf(stdout, "\n"); if (values[n].vlen == 5) { SDP_GET8(type, values[n].value); if (type == SDP_DATA_UINT32) { SDP_GET32(value, values[n].value); fprintf(stdout, "Record Handle: " \ "%#8.8x\n", value); } else fprintf(stderr, "Invalid type=%#x " \ "Record Handle " \ "attribute!\n", type); } else fprintf(stderr, "Invalid size=%d for Record " \ "Handle attribute\n", values[n].vlen); break; case SDP_ATTR_SERVICE_CLASS_ID_LIST: fprintf(stdout, "Service Class ID List:\n"); print_service_class_id_list(values[n].value, values[n].value + values[n].vlen); break; case SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST: fprintf(stdout, "Protocol Descriptor List:\n"); print_protocol_descriptor_list(values[n].value, values[n].value + values[n].vlen); break; case SDP_ATTR_BLUETOOTH_PROFILE_DESCRIPTOR_LIST: fprintf(stdout, "Bluetooth Profile Descriptor List:\n"); print_bluetooth_profile_descriptor_list(values[n].value, values[n].value + values[n].vlen); break; default: fprintf(stderr, "Unexpected attribute ID=%#4.4x\n", values[n].attr); break; } } return (OK); } /* do_sdp_search */ /* Perform SDP browse command */ static int do_sdp_browse(void *xs, int argc, char **argv) { #undef _STR #undef STR #define _STR(x) #x #define STR(x) _STR(x) static char const * const av[] = { STR(SDP_SERVICE_CLASS_PUBLIC_BROWSE_GROUP), NULL }; switch (argc) { case 0: argc = 1; argv = (char **) av; /* FALL THROUGH */ case 1: return (do_sdp_search(xs, argc, argv)); } return (USAGE); } /* do_sdp_browse */ /* List of SDP commands */ struct sdp_command sdp_commands[] = { { "Browse []", "Browse for services. The parameter is a 16-bit UUID of the group\n" \ "to browse. If omitted is set to Public Browse Group.\n\n" \ "\t - xxxx; 16-bit UUID of the group to browse\n", do_sdp_browse }, { "Search ", "Search for the . The parameter is a 16-bit UUID of the\n" \ "service to search for. For some services it is possible to use service name\n"\ "instead of service UUID\n\n" \ "\t - xxxx; 16-bit UUID of the service to search for\n\n" \ "\tKnown service names\n" \ "\t===================\n" \ "\tCIP - Common ISDN Access\n" \ "\tCTP - Cordless Telephony\n" \ "\tDUN - DialUp Networking\n" \ "\tFAX - Fax\n" \ "\tFTRN - OBEX File Transfer\n" \ "\tGN - GN\n" \ "\tHID - Human Interface Device\n" \ "\tHSET - Headset\n" \ "\tLAN - LAN Access Using PPP\n" \ "\tNAP - Network Access Point\n" \ "\tOPUSH - OBEX Object Push\n" \ "\tSP - Serial Port\n", do_sdp_search }, { NULL, NULL, NULL } }; diff --git a/usr.sbin/bluetooth/sdpd/profile.c b/usr.sbin/bluetooth/sdpd/profile.c index 3c6ada6871c7..e6d81f8ef4b1 100644 --- a/usr.sbin/bluetooth/sdpd/profile.c +++ b/usr.sbin/bluetooth/sdpd/profile.c @@ -1,503 +1,504 @@ /* * profile.c */ /*- * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: profile.c,v 1.6 2004/01/13 19:31:54 max Exp $ */ +#include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include "profile.h" #include "provider.h" /* * Lookup profile descriptor */ profile_p profile_get_descriptor(uint16_t uuid) { extern profile_t audio_sink_profile_descriptor; extern profile_t audio_source_profile_descriptor; extern profile_t dun_profile_descriptor; extern profile_t ftrn_profile_descriptor; extern profile_t irmc_profile_descriptor; extern profile_t irmc_command_profile_descriptor; extern profile_t lan_profile_descriptor; extern profile_t opush_profile_descriptor; extern profile_t sp_profile_descriptor; extern profile_t nap_profile_descriptor; extern profile_t gn_profile_descriptor; extern profile_t panu_profile_descriptor; static const profile_p profiles[] = { &audio_sink_profile_descriptor, &audio_source_profile_descriptor, &dun_profile_descriptor, &ftrn_profile_descriptor, &irmc_profile_descriptor, &irmc_command_profile_descriptor, &lan_profile_descriptor, &opush_profile_descriptor, &sp_profile_descriptor, &nap_profile_descriptor, &gn_profile_descriptor, &panu_profile_descriptor }; int32_t i; - for (i = 0; i < sizeof(profiles)/sizeof(profiles[0]); i++) + for (i = 0; i < nitems(profiles); i++) if (profiles[i]->uuid == uuid) return (profiles[i]); return (NULL); } /* * Look attribute in the profile descripror */ profile_attr_create_p profile_get_attr(const profile_p profile, uint16_t attr) { attr_p ad = (attr_p) profile->attrs; for (; ad->create != NULL; ad ++) if (ad->attr == attr) return (ad->create); return (NULL); } /* * uint32 value32 - 5 bytes */ int32_t common_profile_create_service_record_handle( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (buf + 5 > eob) return (-1); SDP_PUT8(SDP_DATA_UINT32, buf); SDP_PUT32(((provider_p) data)->handle, buf); return (5); } /* * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes * [ uuid16 value ] */ int32_t common_profile_create_service_class_id_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { int32_t len = 3 * (datalen >>= 1); if (len <= 0 || len > 0xff || buf + 2 + len > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(len, buf); for (; datalen > 0; datalen --) { SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(*((uint16_t const *)data), buf); data += sizeof(uint16_t); } return (2 + len); } /* * seq8 len8 - 2 bytes * seq 8 len8 - 2 bytes * uuid16 value16 - 3 bytes * uint16 value16 - 3 bytes * [ seq 8 len8 * uuid16 value16 * uint16 value16 ] */ int32_t common_profile_create_bluetooth_profile_descriptor_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { int32_t len = 8 * (datalen >>= 2); if (len <= 0 || len > 0xff || buf + 2 + len > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(len, buf); for (; datalen > 0; datalen --) { SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(6, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(*((uint16_t const *)data), buf); data += sizeof(uint16_t); SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(*((uint16_t const *)data), buf); data += sizeof(uint16_t); } return (2 + len); } /* * seq8 len8 - 2 bytes * uint16 value16 - 3 bytes * uint16 value16 - 3 bytes * uint16 value16 - 3 bytes */ int32_t common_profile_create_language_base_attribute_id_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (buf + 11 > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(9, buf); /* * Language code per ISO 639:1988. Use "en". */ SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(((0x65 << 8) | 0x6e), buf); /* * Encoding. Recommended is UTF-8. ISO639 UTF-8 MIBenum is 106 * (http://www.iana.org/assignments/character-sets) */ SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(106, buf); /* * Offset (Primary Language Base is 0x100) */ SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(SDP_ATTR_PRIMARY_LANGUAGE_BASE_ID, buf); return (11); } /* * Common provider name is "FreeBSD" */ int32_t common_profile_create_service_provider_name( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { char provider_name[] = "FreeBSD"; return (common_profile_create_string8(buf, eob, (uint8_t const *) provider_name, strlen(provider_name))); } /* * str8 len8 string */ int32_t common_profile_create_string8( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (datalen == 0 || datalen > 0xff || buf + 2 + datalen > eob) return (-1); SDP_PUT8(SDP_DATA_STR8, buf); SDP_PUT8(datalen, buf); memcpy(buf, data, datalen); return (2 + datalen); } /* * Service Availability */ int32_t common_profile_create_service_availability( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (datalen != 1 || buf + 2 > eob) return (-1); SDP_PUT8(SDP_DATA_UINT8, buf); SDP_PUT8(data[0], buf); return (2); } /* * seq8 len8 - 2 bytes * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes * uint8 value8 - 2 bytes */ int32_t rfcomm_profile_create_protocol_descriptor_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (datalen != 1 || buf + 14 > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(12, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(3, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_L2CAP, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(5, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_RFCOMM, buf); SDP_PUT8(SDP_DATA_UINT8, buf); SDP_PUT8(*data, buf); return (14); } /* * seq8 len8 - 2 bytes * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes * uint8 value8 - 2 bytes * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes */ int32_t obex_profile_create_protocol_descriptor_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { if (datalen != 1 || buf + 19 > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(17, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(3, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_L2CAP, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(5, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_RFCOMM, buf); SDP_PUT8(SDP_DATA_UINT8, buf); SDP_PUT8(*data, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(3, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_OBEX, buf); return (19); } /* * seq8 len8 * uint8 value8 - bytes * [ uint8 value 8 ] */ int32_t obex_profile_create_supported_formats_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { int32_t len = 2 * datalen; if (len <= 0 || len > 0xff || buf + 2 + len > eob) return (-1); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(len, buf); for (; datalen > 0; datalen --) { SDP_PUT8(SDP_DATA_UINT8, buf); SDP_PUT8(*data++, buf); } return (2 + len); } /* * do not check anything */ int32_t common_profile_always_valid(uint8_t const *data, uint32_t datalen) { return (1); } /* * verify server channel number (the first byte in the data) */ int32_t common_profile_server_channel_valid(uint8_t const *data, uint32_t datalen) { if (data[0] < 1 || data[0] > 30) return (0); return (1); } /* * verify server channel number and supported_formats_size * sdp_opush_profile and sdp_irmc_profile */ int32_t obex_profile_data_valid(uint8_t const *data, uint32_t datalen) { sdp_opush_profile_p opush = (sdp_opush_profile_p) data; if (opush->server_channel < 1 || opush->server_channel > 30 || opush->supported_formats_size == 0 || opush->supported_formats_size > sizeof(opush->supported_formats)) return (0); return (1); } /* * BNEP protocol descriptor */ int32_t bnep_profile_create_protocol_descriptor_list( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { /* supported protocol types */ uint16_t ptype[] = { 0x0800, /* IPv4 */ 0x0806, /* ARP */ #ifdef INET6 0x86dd, /* IPv6 */ #endif }; uint16_t i, psm, version = 0x0100, - nptypes = sizeof(ptype)/sizeof(ptype[0]), + nptypes = nitems(ptype), nptypes_size = nptypes * 3; if (datalen != 2 || 18 + nptypes_size > 255 || buf + 20 + nptypes_size > eob) return (-1); memcpy(&psm, data, sizeof(psm)); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(18 + nptypes_size, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(6, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_L2CAP, buf); SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(psm, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(8 + nptypes_size, buf); SDP_PUT8(SDP_DATA_UUID16, buf); SDP_PUT16(SDP_UUID_PROTOCOL_BNEP, buf); SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(version, buf); SDP_PUT8(SDP_DATA_SEQ8, buf); SDP_PUT8(nptypes_size, buf); for (i = 0; i < nptypes; i ++) { SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(ptype[i], buf); } return (20 + nptypes_size); } /* * BNEP security description */ int32_t bnep_profile_create_security_description( uint8_t *buf, uint8_t const * const eob, uint8_t const *data, uint32_t datalen) { uint16_t security_descr; if (datalen != 2 || buf + 3 > eob) return (-1); memcpy(&security_descr, data, sizeof(security_descr)); SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(security_descr, buf); return (3); } diff --git a/usr.sbin/bluetooth/sdpd/sar.c b/usr.sbin/bluetooth/sdpd/sar.c index 0470109ebc07..68193b567fd3 100644 --- a/usr.sbin/bluetooth/sdpd/sar.c +++ b/usr.sbin/bluetooth/sdpd/sar.c @@ -1,319 +1,320 @@ /*- * sar.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: sar.c,v 1.2 2004/01/08 23:46:51 max Exp $ */ +#include #include #include #include #include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include /* for NULL */ #include "profile.h" #include "provider.h" #include "server.h" /* * Prepare SDP attr/value pair. Check if profile implements the attribute * and if so call the attribute value function. * * uint16 value16 - 3 bytes (attribute) * value - N bytes (value) */ static int32_t server_prepare_attr_value_pair( provider_p const provider, uint16_t attr, uint8_t *buf, uint8_t const * const eob) { profile_attr_create_p cf = profile_get_attr(provider->profile, attr); int32_t len; if (cf == NULL) return (0); /* no attribute */ if (buf + 3 > eob) return (-1); SDP_PUT8(SDP_DATA_UINT16, buf); SDP_PUT16(attr, buf); len = cf(buf, eob, (uint8_t const *) provider, sizeof(*provider)); if (len < 0) return (-1); return (3 + len); } /* * seq16 value16 - 3 bytes * attr value - 3+ bytes * [ attr value ] */ int32_t server_prepare_attr_list(provider_p const provider, uint8_t const *req, uint8_t const * const req_end, uint8_t *rsp, uint8_t const * const rsp_end) { uint8_t *ptr = rsp + 3; int32_t type, hi, lo, len; if (ptr > rsp_end) return (-1); while (req < req_end) { SDP_GET8(type, req); switch (type) { case SDP_DATA_UINT16: if (req + 2 > req_end) return (-1); SDP_GET16(lo, req); hi = lo; break; case SDP_DATA_UINT32: if (req + 4 > req_end) return (-1); SDP_GET16(lo, req); SDP_GET16(hi, req); break; default: return (-1); /* NOT REACHED */ } for (; lo <= hi; lo ++) { len = server_prepare_attr_value_pair(provider, lo, ptr, rsp_end); if (len < 0) return (-1); ptr += len; } } len = ptr - rsp; /* we put this much bytes in rsp */ /* Fix SEQ16 header for the rsp */ SDP_PUT8(SDP_DATA_SEQ16, rsp); SDP_PUT16(len - 3, rsp); return (len); } /* * Prepare SDP Service Attribute Response */ int32_t server_prepare_service_attribute_response(server_p srv, int32_t fd) { uint8_t const *req = srv->req + sizeof(sdp_pdu_t); uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len; uint8_t *rsp = srv->fdidx[fd].rsp; uint8_t const *rsp_end = rsp + NG_L2CAP_MTU_MAXIMUM; uint8_t *ptr = NULL; provider_t *provider = NULL; uint32_t handle; int32_t type, rsp_limit, aidlen, cslen, cs; /* * Minimal Service Attribute Request request * * value32 - 4 bytes ServiceRecordHandle * value16 - 2 bytes MaximumAttributeByteCount * seq8 len8 - 2 bytes * uint16 value16 - 3 bytes AttributeIDList * value8 - 1 byte ContinuationState */ if (req_end - req < 12) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Get ServiceRecordHandle and MaximumAttributeByteCount */ SDP_GET32(handle, req); SDP_GET16(rsp_limit, req); if (rsp_limit <= 0) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Get size of AttributeIDList */ aidlen = 0; SDP_GET8(type, req); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(aidlen, req); break; case SDP_DATA_SEQ16: SDP_GET16(aidlen, req); break; case SDP_DATA_SEQ32: SDP_GET32(aidlen, req); break; } if (aidlen <= 0) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); ptr = (uint8_t *) req + aidlen; /* Get ContinuationState */ if (ptr + 1 > req_end) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); SDP_GET8(cslen, ptr); if (cslen != 0) { if (cslen != 2 || req_end - ptr != 2) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); SDP_GET16(cs, ptr); } else cs = 0; /* Process the request. First, check continuation state */ if (srv->fdidx[fd].rsp_cs != cs) return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE); if (srv->fdidx[fd].rsp_size > 0) return (0); /* Lookup record handle */ if ((provider = provider_by_handle(handle)) == NULL) return (SDP_ERROR_CODE_INVALID_SERVICE_RECORD_HANDLE); /* * Service Attribute Response format * * value16 - 2 bytes AttributeListByteCount (not incl.) * seq8 len16 - 3 bytes * attr value - 3+ bytes AttributeList * [ attr value ] */ cs = server_prepare_attr_list(provider, req, req+aidlen, rsp, rsp_end); if (cs < 0) return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES); /* Set reply size (not counting PDU header and continuation state) */ srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 2; if (srv->fdidx[fd].rsp_limit > rsp_limit) srv->fdidx[fd].rsp_limit = rsp_limit; srv->fdidx[fd].rsp_size = cs; srv->fdidx[fd].rsp_cs = 0; return (0); } /* * Send SDP Service [Search] Attribute Response */ int32_t server_send_service_attribute_response(server_p srv, int32_t fd) { uint8_t *rsp = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_cs; uint8_t *rsp_end = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_size; struct iovec iov[4]; sdp_pdu_t pdu; uint16_t bcount; uint8_t cs[3]; int32_t size; /* First update continuation state (assume we will send all data) */ size = rsp_end - rsp; srv->fdidx[fd].rsp_cs += size; if (size + 1 > srv->fdidx[fd].rsp_limit) { /* * We need to split out response. Add 3 more bytes for the * continuation state and move rsp_end and rsp_cs backwards. */ while ((rsp_end - rsp) + 3 > srv->fdidx[fd].rsp_limit) { rsp_end --; srv->fdidx[fd].rsp_cs --; } cs[0] = 2; cs[1] = srv->fdidx[fd].rsp_cs >> 8; cs[2] = srv->fdidx[fd].rsp_cs & 0xff; } else cs[0] = 0; assert(rsp_end >= rsp); bcount = rsp_end - rsp; if (((sdp_pdu_p)(srv->req))->pid == SDP_PDU_SERVICE_ATTRIBUTE_REQUEST) pdu.pid = SDP_PDU_SERVICE_ATTRIBUTE_RESPONSE; else pdu.pid = SDP_PDU_SERVICE_SEARCH_ATTRIBUTE_RESPONSE; pdu.tid = ((sdp_pdu_p)(srv->req))->tid; pdu.len = htons(sizeof(bcount) + bcount + 1 + cs[0]); bcount = htons(bcount); iov[0].iov_base = &pdu; iov[0].iov_len = sizeof(pdu); iov[1].iov_base = &bcount; iov[1].iov_len = sizeof(bcount); iov[2].iov_base = rsp; iov[2].iov_len = rsp_end - rsp; iov[3].iov_base = cs; iov[3].iov_len = 1 + cs[0]; do { - size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0])); + size = writev(fd, (struct iovec const *) &iov, nitems(iov)); } while (size < 0 && errno == EINTR); /* Check if we have sent (or failed to sent) last response chunk */ if (srv->fdidx[fd].rsp_cs == srv->fdidx[fd].rsp_size) { srv->fdidx[fd].rsp_cs = 0; srv->fdidx[fd].rsp_size = 0; srv->fdidx[fd].rsp_limit = 0; } return ((size < 0)? errno : 0); } diff --git a/usr.sbin/bluetooth/sdpd/srr.c b/usr.sbin/bluetooth/sdpd/srr.c index 8e887a4841de..b16eb25c9ea4 100644 --- a/usr.sbin/bluetooth/sdpd/srr.c +++ b/usr.sbin/bluetooth/sdpd/srr.c @@ -1,141 +1,142 @@ /*- * srr.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: srr.c,v 1.1 2004/01/13 01:54:39 max Exp $ */ +#include #include #include #include #include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include "profile.h" #include "provider.h" #include "server.h" /* * Prepare Service Register response */ int32_t server_prepare_service_register_response(server_p srv, int32_t fd) { uint8_t const *req = srv->req + sizeof(sdp_pdu_t); uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len; uint8_t *rsp = srv->fdidx[fd].rsp; profile_t *profile = NULL; provider_t *provider = NULL; bdaddr_t *bdaddr = NULL; int32_t uuid; /* * Minimal Service Register Request * * value16 - uuid 2 bytes * bdaddr - BD_ADDR 6 bytes */ if (!srv->fdidx[fd].control || !srv->fdidx[fd].priv || req_end - req < 8) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Get ServiceClass UUID */ SDP_GET16(uuid, req); /* Get BD_ADDR */ bdaddr = (bdaddr_p) req; req += sizeof(*bdaddr); /* Lookup profile descriptror */ profile = profile_get_descriptor(uuid); if (profile == NULL) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Validate user data */ if (req_end - req < profile->dsize || profile->valid == NULL || (profile->valid)(req, req_end - req) == 0) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Register provider */ provider = provider_register(profile, bdaddr, fd, req, req_end - req); if (provider == NULL) return (SDP_ERROR_CODE_INSUFFICIENT_RESOURCES); SDP_PUT16(0, rsp); SDP_PUT32(provider->handle, rsp); /* Set reply size */ srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t); srv->fdidx[fd].rsp_size = rsp - srv->fdidx[fd].rsp; srv->fdidx[fd].rsp_cs = 0; return (0); } /* * Send Service Register Response */ int32_t server_send_service_register_response(server_p srv, int32_t fd) { struct iovec iov[2]; sdp_pdu_t pdu; int32_t size; assert(srv->fdidx[fd].rsp_size < srv->fdidx[fd].rsp_limit); pdu.pid = SDP_PDU_ERROR_RESPONSE; pdu.tid = ((sdp_pdu_p)(srv->req))->tid; pdu.len = htons(srv->fdidx[fd].rsp_size); iov[0].iov_base = &pdu; iov[0].iov_len = sizeof(pdu); iov[1].iov_base = srv->fdidx[fd].rsp; iov[1].iov_len = srv->fdidx[fd].rsp_size; do { - size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0])); + size = writev(fd, (struct iovec const *) &iov, nitems(iov)); } while (size < 0 && errno == EINTR); srv->fdidx[fd].rsp_cs = 0; srv->fdidx[fd].rsp_size = 0; srv->fdidx[fd].rsp_limit = 0; return ((size < 0)? errno : 0); } diff --git a/usr.sbin/bluetooth/sdpd/ssr.c b/usr.sbin/bluetooth/sdpd/ssr.c index c6b3a1f65fe1..1a29dde97ee5 100644 --- a/usr.sbin/bluetooth/sdpd/ssr.c +++ b/usr.sbin/bluetooth/sdpd/ssr.c @@ -1,284 +1,285 @@ /*- * ssr.c * * SPDX-License-Identifier: BSD-2-Clause * * Copyright (c) 2004 Maksim Yevmenkin * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. * * $Id: ssr.c,v 1.5 2004/01/13 01:54:39 max Exp $ */ +#include #include #include #include #include #include #define L2CAP_SOCKET_CHECKED #include #include #include #include #include "profile.h" #include "provider.h" #include "server.h" #include "uuid-private.h" /* * Prepare SDP Service Search Response */ int32_t server_prepare_service_search_response(server_p srv, int32_t fd) { uint8_t const *req = srv->req + sizeof(sdp_pdu_t); uint8_t const *req_end = req + ((sdp_pdu_p)(srv->req))->len; uint8_t *rsp = srv->fdidx[fd].rsp; uint8_t const *rsp_end = rsp + NG_L2CAP_MTU_MAXIMUM; uint8_t *ptr = NULL; provider_t *provider = NULL; int32_t type, ssplen, rsp_limit, rcount, cslen, cs; uint128_t uuid, puuid; /* * Minimal SDP Service Search Request * * seq8 len8 - 2 bytes * uuid16 value16 - 3 bytes ServiceSearchPattern * value16 - 2 bytes MaximumServiceRecordCount * value8 - 1 byte ContinuationState */ if (req_end - req < 8) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Get size of ServiceSearchPattern */ ssplen = 0; SDP_GET8(type, req); switch (type) { case SDP_DATA_SEQ8: SDP_GET8(ssplen, req); break; case SDP_DATA_SEQ16: SDP_GET16(ssplen, req); break; case SDP_DATA_SEQ32: SDP_GET32(ssplen, req); break; } if (ssplen <= 0) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); ptr = (uint8_t *) req + ssplen; /* Get MaximumServiceRecordCount */ if (ptr + 2 > req_end) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); SDP_GET16(rsp_limit, ptr); if (rsp_limit <= 0) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* Get ContinuationState */ if (ptr + 1 > req_end) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); SDP_GET8(cslen, ptr); if (cslen != 0) { if (cslen != 2 || req_end - ptr != 2) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); SDP_GET16(cs, ptr); } else cs = 0; /* Process the request. First, check continuation state */ if (srv->fdidx[fd].rsp_cs != cs) return (SDP_ERROR_CODE_INVALID_CONTINUATION_STATE); if (srv->fdidx[fd].rsp_size > 0) return (0); /* * Service Search Response format * * value16 - 2 bytes TotalServiceRecordCount (not incl.) * value16 - 2 bytes CurrentServiceRecordCount (not incl.) * value32 - 4 bytes handle * [ value32 ] * * Calculate how many record handles we can fit * in our reply buffer and adjust rlimit. */ ptr = rsp; rcount = (rsp_end - ptr) / 4; if (rcount < rsp_limit) rsp_limit = rcount; /* Look for the record handles */ for (rcount = 0; ssplen > 0 && rcount < rsp_limit; ) { SDP_GET8(type, req); ssplen --; switch (type) { case SDP_DATA_UUID16: if (ssplen < 2) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); memcpy(&uuid, &uuid_base, sizeof(uuid)); uuid.b[2] = *req ++; uuid.b[3] = *req ++; ssplen -= 2; break; case SDP_DATA_UUID32: if (ssplen < 4) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); memcpy(&uuid, &uuid_base, sizeof(uuid)); uuid.b[0] = *req ++; uuid.b[1] = *req ++; uuid.b[2] = *req ++; uuid.b[3] = *req ++; ssplen -= 4; break; case SDP_DATA_UUID128: if (ssplen < 16) return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); memcpy(uuid.b, req, 16); req += 16; ssplen -= 16; break; default: return (SDP_ERROR_CODE_INVALID_REQUEST_SYNTAX); /* NOT REACHED */ } for (provider = provider_get_first(); provider != NULL && rcount < rsp_limit; provider = provider_get_next(provider)) { if (!provider_match_bdaddr(provider, &srv->req_sa.l2cap_bdaddr)) continue; memcpy(&puuid, &uuid_base, sizeof(puuid)); puuid.b[2] = provider->profile->uuid >> 8; puuid.b[3] = provider->profile->uuid; if (memcmp(&uuid, &puuid, sizeof(uuid)) == 0 || memcmp(&uuid, &uuid_public_browse_group, sizeof(uuid)) == 0) { SDP_PUT32(provider->handle, ptr); rcount ++; } } } /* Set reply size (not counting PDU header and continuation state) */ srv->fdidx[fd].rsp_limit = srv->fdidx[fd].omtu - sizeof(sdp_pdu_t) - 4; srv->fdidx[fd].rsp_size = ptr - rsp; srv->fdidx[fd].rsp_cs = 0; return (0); } /* * Send SDP Service Search Response */ int32_t server_send_service_search_response(server_p srv, int32_t fd) { uint8_t *rsp = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_cs; uint8_t *rsp_end = srv->fdidx[fd].rsp + srv->fdidx[fd].rsp_size; struct iovec iov[4]; sdp_pdu_t pdu; uint16_t rcounts[2]; uint8_t cs[3]; int32_t size; /* First update continuation state (assume we will send all data) */ size = rsp_end - rsp; srv->fdidx[fd].rsp_cs += size; if (size + 1 > srv->fdidx[fd].rsp_limit) { /* * We need to split out response. Add 3 more bytes for the * continuation state and move rsp_end and rsp_cs backwards. */ while ((rsp_end - rsp) + 3 > srv->fdidx[fd].rsp_limit) { rsp_end -= 4; srv->fdidx[fd].rsp_cs -= 4; } cs[0] = 2; cs[1] = srv->fdidx[fd].rsp_cs >> 8; cs[2] = srv->fdidx[fd].rsp_cs & 0xff; } else cs[0] = 0; assert(rsp_end >= rsp); rcounts[0] = srv->fdidx[fd].rsp_size / 4; /* TotalServiceRecordCount */ rcounts[1] = (rsp_end - rsp) / 4; /* CurrentServiceRecordCount */ pdu.pid = SDP_PDU_SERVICE_SEARCH_RESPONSE; pdu.tid = ((sdp_pdu_p)(srv->req))->tid; pdu.len = htons(sizeof(rcounts) + rcounts[1] * 4 + 1 + cs[0]); rcounts[0] = htons(rcounts[0]); rcounts[1] = htons(rcounts[1]); iov[0].iov_base = &pdu; iov[0].iov_len = sizeof(pdu); iov[1].iov_base = rcounts; iov[1].iov_len = sizeof(rcounts); iov[2].iov_base = rsp; iov[2].iov_len = rsp_end - rsp; iov[3].iov_base = cs; iov[3].iov_len = 1 + cs[0]; do { - size = writev(fd, (struct iovec const *) &iov, sizeof(iov)/sizeof(iov[0])); + size = writev(fd, (struct iovec const *) &iov, nitems(iov)); } while (size < 0 && errno == EINTR); /* Check if we have sent (or failed to sent) last response chunk */ if (srv->fdidx[fd].rsp_cs == srv->fdidx[fd].rsp_size) { srv->fdidx[fd].rsp_cs = 0; srv->fdidx[fd].rsp_size = 0; srv->fdidx[fd].rsp_limit = 0; } return ((size < 0)? errno : 0); }