diff --git a/drivers/bluetooth/hci_ath.c b/drivers/bluetooth/hci_ath.c index 0ccf6bf01ed4..5f17ec339fb8 100644 --- a/drivers/bluetooth/hci_ath.c +++ b/drivers/bluetooth/hci_ath.c @@ -50,6 +50,17 @@ struct ath_struct { struct work_struct ctxtsw; }; +#define OP_WRITE_TAG 0x01 + +#define INDEX_BDADDR 0x01 + +struct ath_vendor_cmd { + __u8 opcode; + __le16 index; + __u8 len; + __u8 data[251]; +} __packed; + static int ath_wakeup_ar3k(struct tty_struct *tty) { int status = tty->driver->ops->tiocmget(tty); @@ -144,30 +155,34 @@ static int ath_flush(struct hci_uart *hu) return 0; } -static int ath_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr) +static int ath_vendor_cmd(struct hci_dev *hdev, uint8_t opcode, uint16_t index, + const void *data, size_t dlen) { struct sk_buff *skb; - u8 buf[10]; - int err; + struct ath_vendor_cmd cmd; - buf[0] = 0x01; - buf[1] = 0x01; - buf[2] = 0x00; - buf[3] = sizeof(bdaddr_t); - memcpy(buf + 4, bdaddr, sizeof(bdaddr_t)); + if (dlen > sizeof(cmd.data)) + return -EINVAL; - skb = __hci_cmd_sync(hdev, 0xfc0b, sizeof(buf), buf, HCI_INIT_TIMEOUT); - if (IS_ERR(skb)) { - err = PTR_ERR(skb); - BT_ERR("%s: Change address command failed (%d)", - hdev->name, err); - return err; - } + cmd.opcode = opcode; + cmd.index = cpu_to_le16(index); + cmd.len = dlen; + memcpy(cmd.data, data, dlen); + + skb = __hci_cmd_sync(hdev, 0xfc0b, dlen + 4, &cmd, HCI_INIT_TIMEOUT); + if (IS_ERR(skb)) + return PTR_ERR(skb); kfree_skb(skb); return 0; } +static int ath_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr) +{ + return ath_vendor_cmd(hdev, OP_WRITE_TAG, INDEX_BDADDR, bdaddr, + sizeof(*bdaddr)); +} + static int ath_setup(struct hci_uart *hu) { BT_DBG("hu %p", hu);