diff --git a/usr.sbin/bluetooth/rtlbtfw/Makefile b/usr.sbin/bluetooth/rtlbtfw/Makefile --- a/usr.sbin/bluetooth/rtlbtfw/Makefile +++ b/usr.sbin/bluetooth/rtlbtfw/Makefile @@ -5,5 +5,6 @@ MAN= rtlbtfw.8 LIBADD+= usb SRCS= main.c rtlbt_fw.c rtlbt_hw.c +CFLAGS+= -DRTLBTFW_SUPPORTS_FW_V2 .include diff --git a/usr.sbin/bluetooth/rtlbtfw/main.c b/usr.sbin/bluetooth/rtlbtfw/main.c --- a/usr.sbin/bluetooth/rtlbtfw/main.c +++ b/usr.sbin/bluetooth/rtlbtfw/main.c @@ -315,6 +315,7 @@ const struct rtlbt_id_table *ic; uint8_t rom_version; struct rtlbt_firmware fw, cfg; + struct rtlbt_sec_proj_rp sec_proj; enum rtlbt_fw_type fw_type; uint16_t fw_lmp_subversion; @@ -448,7 +449,20 @@ rtlbt_debug("rom_version = %d", rom_version); /* Load in the firmware */ - r = rtlbt_parse_fwfile_v1(&fw, rom_version); + if (fw_type == RTLBT_FW_TYPE_V1) + r = rtlbt_parse_fwfile_v1(&fw, rom_version); + else { +#ifdef RTLBTFW_SUPPORTS_FW_V2 + if (rtlbt_read_sec_proj(hdl, &sec_proj)) { + rtlbt_err("unable to read sec proj"); + goto shutdown; + } + r = rtlbt_parse_fwfile_v2(&fw, rom_version, sec_proj.key[0]); +#else + rtlbt_error("Unsupported device"); +#endif + } + if (r < 0) { rtlbt_err("Parseing firmware file failed"); goto shutdown; diff --git a/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.h b/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.h --- a/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.h +++ b/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.h @@ -30,7 +30,9 @@ #ifndef __RTLBT_FW_H__ #define __RTLBT_FW_H__ +#include #include +#include #define RTLBT_ROM_LMP_8703B 0x8703 #define RTLBT_ROM_LMP_8723A 0x1200 @@ -41,6 +43,10 @@ #define RTLBT_ROM_LMP_8852A 0x8852 #define RTLBT_ROM_LMP_8851B 0x8851 +#define RTL_PATCH_SNIPPETS 0x01 +#define RTL_PATCH_DUMMY_HEADER 0x02 +#define RTL_PATCH_SECURITY_HEADER 0x03 + enum rtlbt_fw_type { RTLBT_FW_TYPE_UNKNOWN, RTLBT_FW_TYPE_V1, @@ -78,6 +84,38 @@ uint32_t num_sections; } __attribute__ ((packed)); +struct rtlbt_section_header { + uint16_t num; + uint16_t reserved; +} __attribute__ ((packed)); + +struct rtlbt_section { + uint32_t opcode; + uint32_t len; + uint8_t data[]; +} __attribute__ ((packed)); + +struct rtlbt_subsection_header { + uint8_t eco; + uint8_t prio; + union { + uint8_t sectype_data[2]; + uint8_t key_id; + }; + uint32_t len; + uint8_t data[]; +} __attribute__((packed)); + + +struct rtlbt_subsection { + LIST_ENTRY(rtlbt_subsection) entries; + uint8_t prio; + uint32_t len; + uint8_t *data; +}; + +LIST_HEAD(rtlbt_subsection_listhead, rtlbt_subsection); + int rtlbt_fw_read(struct rtlbt_firmware *fw, const char *fwname); void rtlbt_fw_free(struct rtlbt_firmware *fw); char *rtlbt_get_fwname(const char *fw_name, const char *prefix, @@ -87,6 +125,7 @@ enum rtlbt_fw_type rtlbt_get_fw_type(struct rtlbt_firmware *fw, uint16_t *fw_lmp_subversion); int rtlbt_parse_fwfile_v1(struct rtlbt_firmware *fw, uint8_t rom_version); +int rtlbt_parse_fwfile_v2(struct rtlbt_firmware *fw, uint8_t rom_version, uint8_t key); int rtlbt_append_fwfile(struct rtlbt_firmware *fw, struct rtlbt_firmware *opt); #endif diff --git a/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.c b/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.c --- a/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.c +++ b/usr.sbin/bluetooth/rtlbtfw/rtlbt_fw.c @@ -27,8 +27,10 @@ * SUCH DAMAGE. */ +#include #include #include +#include #include #include @@ -41,6 +43,7 @@ #include "rtlbt_fw.h" #include "rtlbt_dbg.h" +#include "rtlbt_hw.h" static const struct rtlbt_id_table rtlbt_ic_id_table[] = { { /* 8723A */ @@ -367,6 +370,157 @@ return (0); } +static void +rtlbt_insert_section(struct rtlbt_subsection_listhead *head, struct rtlbt_subsection *subsec) +{ + struct rtlbt_subsection *cur, *tmp; + + LIST_FOREACH_SAFE(cur, head, entries, tmp) { + if (cur->prio <= subsec->prio) + continue; + LIST_INSERT_BEFORE(cur, subsec, entries); + break; + } +} + +static void +rtlbt_free_sections(struct rtlbt_subsection_listhead *head) +{ + struct rtlbt_subsection *cur, *tmp; + + LIST_FOREACH_SAFE(cur, head, entries, tmp) { + LIST_REMOVE(cur, entries); + free(cur); + } +} + +__always_inline static int +rtlbt_prase_fwfile_section(struct rtlbt_subsection_listhead *head, + uint8_t *data, size_t len, uint8_t opcode, + uint8_t rom_version, uint8_t key) +{ + struct rtlbt_section_header *hdr; + struct rtlbt_subsection_header *subsec_hdr; + struct rtlbt_subsection *subsection; + uint16_t num_subsections; + uint32_t subsection_length; + uint8_t *data_base; + int ret = 0; + int i; + + if (len < sizeof(struct rtlbt_section_header)) + return (-1); + hdr = (struct rtlbt_section_header *) data; + num_subsections = le16toh(hdr->num); + + if (len < (sizeof(struct rtlbt_section_header) + + num_subsections * sizeof(struct rtlbt_subsection_header))) + return (-1); + + data_base = data + sizeof(struct rtlbt_section_header); + + for (i = 0; i < num_subsections && (data_base - data) < (long)len; ++i) { + subsec_hdr = (struct rtlbt_subsection_header *) data_base; + subsection_length = le32toh(subsec_hdr->len); + rtlbt_debug("subsection, length %08x", subsection_length); + if (subsec_hdr->eco != (rom_version + 1)) + continue; + if (opcode == RTL_PATCH_SECURITY_HEADER) { + if(subsec_hdr->key_id != key) + continue; + break; + } + subsection = malloc(sizeof(struct rtlbt_subsection)); + subsection->data = data_base + sizeof(struct rtlbt_subsection_header); + subsection->len = subsection_length; + subsection->prio = subsec_hdr->prio; + rtlbt_insert_section(head, subsection); + data_base += sizeof(struct rtlbt_subsection_header) + subsection_length; + ret += subsection_length; + } + if ((data_base - data) >= (long)len) + return (-1); + + return (ret); +} + +int +rtlbt_parse_fwfile_v2(struct rtlbt_firmware *fw, uint8_t rom_version, uint8_t key) +{ + struct rtlbt_fw_header_v2 *fw_header; + struct rtlbt_section *section; + struct rtlbt_subsection_listhead head = LIST_HEAD_INITIALIZER(head); + struct rtlbt_subsection *subsec; + uint8_t *patch_buf, *patch_cur; + unsigned int i; + uint8_t *chip_data_base; + uint32_t num_sections; + uint32_t section_length; + uint32_t opcode; + uint32_t patch_length = 0, ret; + + fw_header = (struct rtlbt_fw_header_v2 *)fw->buf; + num_sections = le32toh(fw_header->num_sections); + rtlbt_debug("fw_version=%lx, num_sections=%d,key=%x", + le64toh(fw_header->fw_version), num_sections + , key); + + /* Find a right patch for the chip. */ + if (fw->len < sizeof(struct rtlbt_fw_header_v2) + + sizeof(fw_ext_sig) + sizeof(struct rtlbt_section) * num_sections) { + errno = EINVAL; + return (-1); + } + + chip_data_base = fw->buf + sizeof(struct rtlbt_fw_header_v2); + for (i = 0; i < num_sections; i++) { + section = (struct rtlbt_section *)chip_data_base; + section_length = le32toh(section->len); + opcode = le32toh(section->opcode); + ret = rtlbt_prase_fwfile_section(&head, chip_data_base + + sizeof(struct rtlbt_section), section_length, + opcode, rom_version, key); + if (ret < 0) { + rtlbt_debug("failed on parsing section %x: %d", + opcode, ret); + rtlbt_free_sections(&head); + errno = EINVAL; + return (-1); + } + patch_length += ret; + chip_data_base += sizeof(struct rtlbt_section) + section_length; + } + + if (fw->len <= (size_t)(chip_data_base - fw->buf)) { + errno = EINVAL; + rtlbt_free_sections(&head); + return (-1); + } + if (patch_length == 0) { + rtlbt_err("can not find patch for chip id %d", rom_version); + errno = EINVAL; + return (-1); + } + + patch_cur = patch_buf = malloc(patch_length); + if (patch_buf == NULL) { + errno = ENOMEM; + return (-1); + } + + LIST_FOREACH(subsec, &head, entries) { + memcpy(patch_cur, subsec->data, subsec->len); + patch_cur += subsec->len; + } + + rtlbt_free_sections(&head); + free(fw->buf); + fw->buf = patch_buf; + fw->len = patch_length; + + return (0); +} + int rtlbt_append_fwfile(struct rtlbt_firmware *fw, struct rtlbt_firmware *opt) { diff --git a/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.h b/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.h --- a/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.h +++ b/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.h @@ -28,6 +28,8 @@ #ifndef __RTLBT_HW_H__ #define __RTLBT_HW_H__ +#include +#include #include /* USB control request (HCI command) structure */ @@ -85,6 +87,11 @@ uint8_t version; } __attribute__ ((packed)); +struct rtlbt_sec_proj_rp { + uint8_t check; + uint8_t key[2]; +} __attribute__ ((packed)); + struct rtlbt_hci_dl_cmd { uint8_t index; uint8_t data[RTLBT_MAX_CMD_DATA_LEN]; @@ -95,6 +102,8 @@ uint8_t index; } __attribute__ ((packed)); +int rtlbt_read_sec_proj(struct libusb_device_handle *hdl, + struct rtlbt_sec_proj_rp *ver); int rtlbt_read_local_ver(struct libusb_device_handle *hdl, ng_hci_read_local_ver_rp *ver); int rtlbt_read_rom_ver(struct libusb_device_handle *hdl, uint8_t *ver); diff --git a/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.c b/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.c --- a/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.c +++ b/usr.sbin/bluetooth/rtlbtfw/rtlbt_hw.c @@ -110,6 +110,42 @@ return (LIBUSB_ERROR_TIMEOUT); } + +int +rtlbt_read_sec_proj(struct libusb_device_handle *hdl, + struct rtlbt_sec_proj_rp *rp) +{ + int ret, transferred; + struct rtlbt_hci_event_cmd_compl *event; + static struct rtlbt_hci_cmd cmd = { + .opcode = htole16(0xfc61), + .length = 5, + .data = {0x10, 0xA4, 0x0D, 0x00, 0xb0}, + }; + uint8_t buf[RTLBT_HCI_EVT_COMPL_SIZE(struct rtlbt_sec_proj_rp)]; + + memset(buf, 0, sizeof(buf)); + + ret = rtlbt_hci_command(hdl, + &cmd, + buf, + sizeof(buf), + &transferred, + RTLBT_HCI_CMD_TIMEOUT); + + if (ret < 0 || transferred != sizeof(buf)) { + rtlbt_debug("Can't read sec_proj key: code=%d, size=%d", + ret, + transferred); + return (-1); + } + + event = (struct rtlbt_hci_event_cmd_compl *)buf; + memcpy(rp->key, event->data, sizeof(struct rtlbt_sec_proj_rp)); + + return (0); +} + int rtlbt_read_local_ver(struct libusb_device_handle *hdl, ng_hci_read_local_ver_rp *ver)