├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── doc └── 50-rpmsg.rules ├── router ├── app_cmds.c ├── circ_buf.c ├── circ_buf.h ├── common_cmds.c ├── diag.c ├── diag.h ├── diag_cntl.c ├── diag_cntl.h ├── dm.c ├── dm.h ├── hdlc.c ├── hdlc.h ├── list.h ├── masks.c ├── masks.h ├── mbuf.c ├── mbuf.h ├── peripheral-qrtr.c ├── peripheral-qrtr.h ├── peripheral-rpmsg.c ├── peripheral-rpmsg.h ├── peripheral.c ├── peripheral.h ├── router.c ├── socket.c ├── uart.c ├── unix.c ├── usb.c ├── util.c ├── util.h ├── watch.c └── watch.h └── tools └── send_data.c /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | diag-router 3 | send_data 4 | cscope.out 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * Copyright (c) 2016, Bjorn Andersson 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | HAVE_LIBUDEV=1 2 | HAVE_LIBQRTR=1 3 | 4 | .PHONY: all 5 | 6 | DIAG := diag-router 7 | SEND_DATA := send_data 8 | 9 | all: $(DIAG) $(SEND_DATA) 10 | 11 | CFLAGS ?= -Wall -g -O2 12 | ifeq ($(HAVE_LIBUDEV),1) 13 | CFLAGS += -DHAS_LIBUDEV=1 14 | LDFLAGS += -ludev 15 | endif 16 | ifeq ($(HAVE_LIBQRTR),1) 17 | CFLAGS += -DHAS_LIBQRTR=1 18 | LDFLAGS += -lqrtr 19 | endif 20 | 21 | SRCS := router/app_cmds.c \ 22 | router/circ_buf.c \ 23 | router/common_cmds.c \ 24 | router/diag.c \ 25 | router/diag_cntl.c \ 26 | router/dm.c \ 27 | router/hdlc.c \ 28 | router/masks.c \ 29 | router/mbuf.c \ 30 | router/peripheral.c \ 31 | router/router.c \ 32 | router/socket.c \ 33 | router/uart.c \ 34 | router/unix.c \ 35 | router/usb.c \ 36 | router/util.c \ 37 | router/watch.c 38 | 39 | ifeq ($(HAVE_LIBUDEV),1) 40 | SRCS += router/peripheral-rpmsg.c 41 | endif 42 | 43 | ifeq ($(HAVE_LIBQRTR),1) 44 | SRCS += router/peripheral-qrtr.c 45 | endif 46 | 47 | OBJS := $(SRCS:.c=.o) 48 | 49 | $(DIAG): $(OBJS) 50 | $(CC) -o $@ $^ $(LDFLAGS) 51 | 52 | SEND_DATA_SRCS := tools/send_data.c 53 | SEND_DATA_OBJS := $(SEND_DATA_SRCS:.c=.o) 54 | 55 | $(SEND_DATA): $(SEND_DATA_OBJS) 56 | $(CC) -o $@ $^ $(LDFLAGS) 57 | 58 | install: $(DIAG) $(SEND_DATA) 59 | install -D -m 755 $(DIAG) $(DESTDIR)$(prefix)/bin/$(DIAG) 60 | install -D -m 755 $(SEND_DATA) $(DESTDIR)$(prefix)/bin/$(SEND_DATA) 61 | 62 | clean: 63 | rm -f $(DIAG) $(OBJS) $(SEND_DATA) $(SEND_DATA_OBJS) 64 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## DIAG 2 | 3 | **DIAG** implements routing of diagnostics related messages between host and 4 | various subsystems. 5 | 6 | ### USB Gadget 7 | 8 | Ensure that your kernel is built with **CONFIG_CONFIGFS_FS** and **CONFIG_USB_CONFIGFS_F_FS** and that configfs is mounted in ```/sys/kernel/config```. 9 | 10 | G1="/sys/kernel/config/usb_gadget/g1" 11 | 12 | mkdir $G1 13 | mkdir $G1/strings/0x409 14 | mkdir $G1/functions/ffs.diag 15 | mkdir $G1/configs/c.1 16 | mkdir $G1/configs/c.1/strings/0x409 17 | 18 | echo 0xVID > $G1/idVendor 19 | echo 0xPID > $G1/idProduct 20 | echo SERIAL > $G1/strings/0x409/serialnumber 21 | echo MANUFACTURER > $G1/strings/0x409/manufacturer 22 | echo PRODUCT > $G1/strings/0x409/product 23 | echo "diag_dun" > $G1/configs/c.1/strings/0x409/configuration 24 | ln -s $G1/functions/ffs.diag $G1/configs/c.1 25 | 26 | mkdir /dev/ffs-diag 27 | mount -t functionfs diag /dev/ffs-diag 28 | 29 | diag-router & 30 | 31 | sleep 1 32 | 33 | echo 6a00000.dwc3 > $G1/UDC 34 | -------------------------------------------------------------------------------- /doc/50-rpmsg.rules: -------------------------------------------------------------------------------- 1 | SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="?*", SYMLINK+="rpmsg/$attr{rpmsg_name}/ctrl" 2 | SUBSYSTEM=="rpmsg", KERNEL=="rpmsg[0-9]*", ATTR{name}=="?*", ATTRS{rpmsg_name}=="?*", SYMLINK+="rpmsg/$attr{rpmsg_name}/$attr{name}" 3 | 4 | ACTION=="add", SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="pronto", RUN+="/usr/bin/rpmsgexport /dev/$name APPS_RIVA_CTRL" 5 | ACTION=="add", SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="pronto", RUN+="/usr/bin/rpmsgexport /dev/$name APPS_RIVA_DATA" 6 | 7 | ACTION=="add", SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="hexagon", RUN+="/usr/bin/rpmsgexport /dev/$name DIAG" 8 | ACTION=="add", SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="hexagon", RUN+="/usr/bin/rpmsgexport /dev/$name DIAG_CNTL" 9 | ACTION=="add", SUBSYSTEM=="rpmsg", KERNEL=="rpmsg_ctrl[0-9]*", ATTRS{rpmsg_name}=="hexagon", RUN+="/usr/bin/rpmsgexport /dev/$name DIAG_CMD" 10 | -------------------------------------------------------------------------------- /router/app_cmds.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "diag.h" 38 | #include "diag_cntl.h" 39 | #include "dm.h" 40 | #include "hdlc.h" 41 | #include "util.h" 42 | 43 | #define DIAG_CMD_KEEP_ALIVE_SUBSYS 50 44 | #define DIAG_CMD_KEEP_ALIVE_CMD 3 45 | 46 | #define DIAG_CMD_DIAG_VERSION_ID 28 47 | #define DIAG_PROTOCOL_VERSION_NUMBER 2 48 | 49 | #define DIAG_CMD_EXTENDED_BUILD_ID 124 50 | #define DIAG_CMD_DIAG_VERSION_NO 0 51 | #define MOBILE_MODEL_NUMBER 0 52 | #define MOBILE_SOFTWARE_REVISION "OE" 53 | #define MOBILE_MODEL_STRING "DB410C" 54 | #define MSM_REVISION_NUMBER 2 55 | 56 | #define DIAG_CMD_DIAG_SUBSYS 18 57 | #define DIAG_CMD_DIAG_GET_DIAG_ID 0x222 58 | 59 | static int handle_diag_version(struct diag_client *client, const void *buf, 60 | size_t len) 61 | { 62 | uint8_t resp[] = { DIAG_CMD_DIAG_VERSION_ID, DIAG_PROTOCOL_VERSION_NUMBER }; 63 | 64 | return dm_send(client, resp, sizeof(resp)); 65 | } 66 | 67 | static int handle_diag_version_no(struct diag_client *client, const void *buf, 68 | size_t len) 69 | { 70 | uint8_t resp[55]; 71 | 72 | memset(resp, 0, 55); 73 | 74 | return dm_send(client, resp, sizeof(resp)); 75 | } 76 | 77 | static int handle_extended_build_id(struct diag_client *client, 78 | const void *buf, size_t len) 79 | { 80 | struct { 81 | uint8_t cmd_code; 82 | uint8_t ver; 83 | uint16_t reserved; 84 | uint32_t msm_rev; 85 | uint32_t mobile_model_number; 86 | char strings[]; 87 | } __packed *resp; 88 | size_t resp_size; 89 | size_t string1_size = strlen(MOBILE_SOFTWARE_REVISION) + 1; 90 | size_t string2_size = strlen(MOBILE_MODEL_STRING) + 1; 91 | size_t strings_size = string1_size + string2_size; 92 | 93 | if (len != sizeof(uint8_t)) 94 | return -EMSGSIZE; 95 | 96 | resp_size = sizeof(*resp) + strings_size; 97 | 98 | resp = alloca(resp_size); 99 | memset(resp, 0, resp_size); 100 | 101 | resp->cmd_code = DIAG_CMD_EXTENDED_BUILD_ID; 102 | resp->ver = DIAG_PROTOCOL_VERSION_NUMBER; 103 | resp->msm_rev = MSM_REVISION_NUMBER; 104 | resp->mobile_model_number = MOBILE_MODEL_NUMBER; 105 | strcpy(resp->strings, MOBILE_SOFTWARE_REVISION); 106 | strcpy(resp->strings + string1_size, MOBILE_MODEL_STRING); 107 | 108 | return dm_send(client, resp, resp_size); 109 | } 110 | 111 | static int handle_keep_alive(struct diag_client *client, const void *buf, 112 | size_t len) 113 | { 114 | uint8_t resp[16]; 115 | 116 | resp[0] = DIAG_CMD_SUBSYS_DISPATCH; 117 | resp[1] = DIAG_CMD_KEEP_ALIVE_SUBSYS; 118 | resp[2] = DIAG_CMD_KEEP_ALIVE_CMD; 119 | memset(resp + 3, 0, sizeof(resp) - 3); 120 | 121 | return dm_send(client, resp, sizeof(resp)); 122 | } 123 | 124 | static int handle_diag_id(struct diag_client *client, const void *buf, size_t len) 125 | { 126 | struct diag_id_query_req { 127 | uint8_t cmd_code; 128 | uint8_t subsys_id; 129 | uint16_t subsys_cmd_code; 130 | uint8_t version; 131 | } __packed; 132 | struct diag_id_query_resp { 133 | struct diag_id_query_req req_info; 134 | uint8_t num_entries; 135 | uint8_t payload[]; 136 | } __packed; 137 | struct diag_id_tbl_t *diag_id_item = NULL; 138 | struct list_head *diag_ids_head = NULL; 139 | uint8_t resp_buffer[DIAG_MAX_RSP_SIZE] = {0}; 140 | uint8_t *offset_resp; 141 | size_t resp_len = 0; 142 | int num_entries = 0; 143 | 144 | if (!buf || len < sizeof(struct diag_id_query_req)) 145 | return -EMSGSIZE; 146 | 147 | struct diag_id_query_req *req = (struct diag_id_query_req *)buf; 148 | struct diag_id_query_resp *resp = (struct diag_id_query_resp *)resp_buffer; 149 | memcpy(resp_buffer, req, sizeof(struct diag_id_query_req)); 150 | offset_resp = (uint8_t *)resp_buffer; 151 | resp_len = offsetof(struct diag_id_query_resp, payload); 152 | 153 | diag_ids_head = diag_get_diag_ids_head(); 154 | list_for_each_entry(diag_id_item, diag_ids_head, node) { 155 | if (resp_len >= DIAG_MAX_RSP_SIZE) 156 | break; 157 | memcpy(offset_resp + resp_len, &diag_id_item->diagid_info, diag_id_item->diag_id_info_len); 158 | resp_len += diag_id_item->diag_id_info_len; 159 | num_entries++; 160 | } 161 | resp->num_entries = num_entries; 162 | 163 | return dm_send(client, resp_buffer, resp_len); 164 | } 165 | 166 | void register_app_cmds(void) 167 | { 168 | register_fallback_cmd(DIAG_CMD_DIAG_VERSION_ID, handle_diag_version); 169 | register_fallback_cmd(DIAG_CMD_DIAG_VERSION_NO, handle_diag_version_no); 170 | register_fallback_cmd(DIAG_CMD_EXTENDED_BUILD_ID, handle_extended_build_id); 171 | register_fallback_subsys_cmd(DIAG_CMD_KEEP_ALIVE_SUBSYS, 172 | DIAG_CMD_KEEP_ALIVE_CMD, handle_keep_alive); 173 | register_fallback_subsys_cmd(DIAG_CMD_DIAG_SUBSYS, 174 | DIAG_CMD_DIAG_GET_DIAG_ID, handle_diag_id); 175 | } 176 | -------------------------------------------------------------------------------- /router/circ_buf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | 35 | #include "circ_buf.h" 36 | 37 | /** 38 | * circ_read() - read data into circular buffer 39 | * @fd: non-blocking file descriptor to read 40 | * @buf: circ_buf object to write to 41 | * 42 | * Return: 0 if fifo is full or fd depleted, negative errno on failure 43 | */ 44 | ssize_t circ_read(int fd, struct circ_buf *buf) 45 | { 46 | size_t space; 47 | ssize_t n; 48 | 49 | do { 50 | space = CIRC_SPACE_TO_END(buf); 51 | if (!space) 52 | return 0; 53 | 54 | n = read(fd, buf->buf + buf->head, space); 55 | if (n < 0) 56 | return n; 57 | 58 | buf->head = (buf->head + n) & (HDLC_BUF_SIZE - 1); 59 | } while (n == space); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /router/circ_buf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __CIRC_BUF_H__ 32 | #define __CIRC_BUF_H__ 33 | 34 | #include "util.h" 35 | 36 | #define HDLC_BUF_SIZE 16384 37 | 38 | struct circ_buf { 39 | char buf[HDLC_BUF_SIZE]; 40 | size_t head; 41 | size_t tail; 42 | }; 43 | 44 | #define CIRC_SPACE(buf) (((buf)->tail - (buf)->head - 1) & (HDLC_BUF_SIZE - 1)) 45 | 46 | #define CIRC_SPACE_TO_END(buf) MIN(CIRC_SPACE(buf), HDLC_BUF_SIZE - (buf)->head) 47 | 48 | ssize_t circ_read(int fd, struct circ_buf *buf); 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /router/common_cmds.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include "diag.h" 38 | #include "dm.h" 39 | #include "hdlc.h" 40 | #include "masks.h" 41 | #include "peripheral.h" 42 | #include "util.h" 43 | 44 | struct diag_log_cmd_mask { 45 | uint32_t equip_id; 46 | uint32_t num_items; 47 | uint8_t mask[0]; 48 | }__packed; 49 | 50 | #define DIAG_CMD_STATUS_SUCCESS 0 51 | #define DIAG_CMD_STATUS_INVALID_EQUIPMENT_ID 1 52 | 53 | #define DIAG_CMD_LOGGING_CONFIGURATION 0x73 54 | #define DIAG_CMD_OP_LOG_DISABLE 0 55 | #define DIAG_CMD_OP_GET_LOG_RANGE 1 56 | #define DIAG_CMD_OP_SET_LOG_MASK 3 57 | #define DIAG_CMD_OP_GET_LOG_MASK 4 58 | 59 | #define DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION 0x7d 60 | #define DIAG_CMD_OP_GET_SSID_RANGE 1 61 | #define DIAG_CMD_OP_GET_BUILD_MASK 2 62 | #define DIAG_CMD_OP_GET_MSG_MASK 3 63 | #define DIAG_CMD_OP_SET_MSG_MASK 4 64 | #define DIAG_CMD_OP_SET_ALL_MSG_MASK 5 65 | 66 | #define DIAG_CMD_MSG_STATUS_UNSUCCESSFUL 0 67 | #define DIAG_CMD_MSG_STATUS_SUCCESSFUL 1 68 | 69 | #define DIAG_CMD_GET_MASK 0x81 70 | #define DIAG_CMD_EVENT_ERROR_CODE_OK 0 71 | #define DIAG_CMD_EVENT_ERROR_CODE_FAIL 1 72 | 73 | #define DIAG_CMD_SET_MASK 0x82 74 | 75 | #define DIAG_CMD_EVENT_REPORT_CONTROL 0x60 76 | 77 | static int handle_logging_configuration(struct diag_client *client, 78 | const void *buf, size_t len) 79 | { 80 | const struct diag_log_cmd_header { 81 | uint8_t cmd_code; 82 | uint8_t reserved[3]; 83 | uint32_t operation; 84 | }__packed *request_header = buf; 85 | 86 | switch (request_header->operation) { 87 | case DIAG_CMD_OP_LOG_DISABLE: { 88 | struct { 89 | struct diag_log_cmd_header header; 90 | uint32_t status; 91 | } __packed resp; 92 | 93 | if (sizeof(*request_header) != len) 94 | return -EMSGSIZE; 95 | 96 | memcpy(&resp, request_header, sizeof(*request_header)); 97 | diag_cmd_disable_log(); 98 | resp.status = DIAG_CMD_STATUS_SUCCESS; 99 | 100 | peripheral_broadcast_log_mask(0); 101 | 102 | dm_send(client, &resp, sizeof(resp)); 103 | break; 104 | } 105 | case DIAG_CMD_OP_GET_LOG_RANGE: { 106 | struct { 107 | struct diag_log_cmd_header header; 108 | uint32_t status; 109 | uint32_t ranges[MAX_EQUIP_ID]; 110 | } __packed resp; 111 | 112 | if (sizeof(*request_header) != len) 113 | return -EMSGSIZE; 114 | 115 | memcpy(&resp, request_header, sizeof(*request_header)); 116 | diag_cmd_get_log_range(resp.ranges, MAX_EQUIP_ID); 117 | resp.status = DIAG_CMD_STATUS_SUCCESS; 118 | 119 | dm_send(client, &resp, sizeof(resp)); 120 | break; 121 | } 122 | case DIAG_CMD_OP_SET_LOG_MASK: { 123 | struct diag_log_cmd_mask *mask_to_set = (struct diag_log_cmd_mask*)(buf + sizeof(struct diag_log_cmd_header)); 124 | struct { 125 | struct diag_log_cmd_header header; 126 | uint32_t status; 127 | struct diag_log_cmd_mask mask_structure; 128 | } __packed *resp; 129 | uint32_t resp_size = sizeof(*resp); 130 | uint32_t mask_size = sizeof(*mask_to_set) + BITS_TO_BYTES(mask_to_set->num_items); 131 | uint32_t num_items; 132 | 133 | if (sizeof(*request_header) + mask_size != len) 134 | return -EMSGSIZE; 135 | 136 | resp_size += mask_size; 137 | resp = malloc(resp_size); 138 | if (!resp) { 139 | warn("Failed to allocate response packet\n"); 140 | return -errno; 141 | } 142 | memcpy(resp, request_header, sizeof(*request_header)); 143 | num_items = mask_to_set->num_items; 144 | diag_cmd_set_log_mask(mask_to_set->equip_id, &num_items, mask_to_set->mask, &mask_size); 145 | mask_to_set->num_items = num_items; 146 | memcpy(&resp->mask_structure, mask_to_set, mask_size); // num_items might have been capped!!! 147 | resp->status = DIAG_CMD_STATUS_SUCCESS; 148 | 149 | peripheral_broadcast_log_mask(resp->mask_structure.equip_id); 150 | 151 | dm_send(client, resp, resp_size); 152 | free(resp); 153 | 154 | break; 155 | } 156 | case DIAG_CMD_OP_GET_LOG_MASK: { 157 | uint32_t *equip_id = (uint32_t *)(buf + sizeof(struct diag_log_cmd_header)); 158 | struct get_log_response_resp { 159 | struct diag_log_cmd_header header; 160 | uint32_t status; 161 | struct diag_log_cmd_mask mask_structure; 162 | } __packed *resp; 163 | uint32_t num_items = 0; 164 | uint8_t *mask; 165 | uint32_t mask_size = 0; 166 | uint32_t resp_size = sizeof(*resp); 167 | 168 | if (sizeof(*request_header) + sizeof(*equip_id) != len) 169 | return -EMSGSIZE; 170 | 171 | if (diag_cmd_get_log_mask(*equip_id, &num_items, &mask, &mask_size) == 0) { 172 | resp_size += mask_size; 173 | resp = malloc(resp_size); 174 | if (!resp) { 175 | warn("Failed to allocate response packet\n"); 176 | return -errno; 177 | } 178 | memcpy(resp, request_header, sizeof(*request_header)); 179 | resp->mask_structure.equip_id = *equip_id; 180 | resp->mask_structure.num_items = num_items; 181 | if (mask != NULL) { 182 | memcpy(&resp->mask_structure.mask, mask, mask_size); 183 | free(mask); 184 | } 185 | resp->status = DIAG_CMD_STATUS_SUCCESS; 186 | } else { 187 | resp = malloc(resp_size); 188 | if (!resp) { 189 | warn("Failed to allocate response packet\n"); 190 | return -errno; 191 | } 192 | memcpy(resp, request_header, sizeof(*request_header)); 193 | resp->mask_structure.equip_id = *equip_id; 194 | resp->mask_structure.num_items = num_items; 195 | resp->status = DIAG_CMD_STATUS_INVALID_EQUIPMENT_ID; 196 | } 197 | 198 | peripheral_broadcast_log_mask(resp->mask_structure.equip_id); 199 | 200 | dm_send(client, resp, resp_size); 201 | free(resp); 202 | 203 | break; 204 | } 205 | default: 206 | warn("Unrecognized operation %d!!!", request_header->operation); 207 | return -EINVAL; 208 | } 209 | 210 | return 0; 211 | } 212 | 213 | static int handle_extended_message_configuration(struct diag_client *client, 214 | const void *buf, size_t len) 215 | { 216 | const struct diag_msg_cmd_header { 217 | uint8_t cmd_code; 218 | uint8_t operation; 219 | }__packed *request_header = buf; 220 | 221 | switch (request_header->operation) { 222 | case DIAG_CMD_OP_GET_SSID_RANGE: { 223 | struct { 224 | struct diag_msg_cmd_header header; 225 | uint8_t status; 226 | uint8_t reserved; 227 | uint32_t range_cnt; 228 | struct diag_ssid_range_t ranges[]; 229 | } __packed *resp; 230 | uint32_t resp_size = sizeof(*resp); 231 | uint32_t count = 0; 232 | struct diag_ssid_range_t *ranges = NULL; 233 | uint32_t ranges_size = 0; 234 | 235 | if (sizeof(*request_header) != len) 236 | return -EMSGSIZE; 237 | 238 | diag_cmd_get_ssid_range(&count, &ranges); 239 | ranges_size = count * sizeof(*ranges); 240 | resp_size += ranges_size; 241 | resp = malloc(resp_size); 242 | if (!resp) { 243 | warn("Failed to allocate response packet\n"); 244 | return -errno; 245 | } 246 | memcpy(resp, request_header, sizeof(*request_header)); 247 | resp->range_cnt = count; 248 | if (ranges != NULL) { 249 | memcpy(resp->ranges, ranges, ranges_size); 250 | free(ranges); 251 | } 252 | resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL; 253 | 254 | dm_send(client, resp, resp_size); 255 | free(resp); 256 | 257 | break; 258 | } 259 | case DIAG_CMD_OP_GET_BUILD_MASK: { 260 | struct diag_ssid_range_t range; 261 | struct { 262 | uint8_t cmd; 263 | uint8_t subcmd; 264 | struct diag_ssid_range_t range; 265 | uint8_t status; 266 | uint8_t reserved; 267 | uint32_t masks[]; 268 | } __packed *resp; 269 | uint32_t resp_size = sizeof(*resp); 270 | uint32_t *masks; 271 | uint32_t masks_size; 272 | 273 | if (sizeof(*request_header) + sizeof(range) != len) 274 | return -EMSGSIZE; 275 | 276 | memcpy(&range, buf + sizeof(struct diag_msg_cmd_header), sizeof(range)); 277 | 278 | if (diag_cmd_get_build_mask(&range, &masks) == 0) { 279 | masks_size = MSG_RANGE_TO_SIZE(range); 280 | resp_size += masks_size; 281 | 282 | resp = alloca(resp_size); 283 | memset(resp, 0, resp_size); 284 | resp->cmd = DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION; 285 | resp->subcmd = DIAG_CMD_OP_GET_BUILD_MASK; 286 | 287 | resp->range.ssid_first = range.ssid_first; 288 | resp->range.ssid_last = range.ssid_last; 289 | 290 | resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL; 291 | memcpy(resp->masks, masks, masks_size); 292 | 293 | free(masks); 294 | } else { 295 | resp = alloca(resp_size); 296 | memset(resp, 0, resp_size); 297 | resp->cmd = DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION; 298 | resp->subcmd = DIAG_CMD_OP_GET_BUILD_MASK; 299 | resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL; 300 | } 301 | 302 | dm_send(client, resp, resp_size); 303 | 304 | break; 305 | } 306 | case DIAG_CMD_OP_GET_MSG_MASK: { 307 | struct diag_ssid_range_t range; 308 | struct { 309 | struct diag_msg_cmd_header header; 310 | uint8_t status; 311 | uint8_t rsvd; 312 | uint32_t rt_masks[]; 313 | } __packed *resp; 314 | uint32_t resp_size = sizeof(*resp); 315 | uint32_t *masks = NULL; 316 | uint32_t masks_size = 0; 317 | 318 | if (sizeof(*request_header) + sizeof(range) != len) 319 | return -EMSGSIZE; 320 | 321 | memcpy(&range, buf + sizeof(struct diag_msg_cmd_header), sizeof(range)); 322 | 323 | if (diag_cmd_get_msg_mask(&range, &masks) == 0) { 324 | masks_size = MSG_RANGE_TO_SIZE(range); 325 | resp_size += masks_size; 326 | 327 | resp = alloca(resp_size); 328 | memset(resp, 0, resp_size); 329 | 330 | memcpy(resp, request_header, sizeof(*request_header)); 331 | memcpy(resp->rt_masks, masks, masks_size); 332 | resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL; 333 | } else { 334 | resp = alloca(resp_size); 335 | memset(resp, 0, resp_size); 336 | 337 | memcpy(resp, request_header, sizeof(*request_header)); 338 | resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL; 339 | } 340 | 341 | dm_send(client, resp, resp_size); 342 | free(masks); 343 | 344 | break; 345 | } 346 | case DIAG_CMD_OP_SET_MSG_MASK: { 347 | const struct { 348 | struct diag_msg_cmd_header header; 349 | struct diag_ssid_range_t range; 350 | uint16_t rsvd; 351 | uint32_t masks[]; 352 | } __packed *req = buf; 353 | struct { 354 | struct diag_msg_cmd_header header; 355 | struct diag_ssid_range_t range; 356 | uint8_t status; 357 | uint8_t rsvd; 358 | uint32_t rt_masks[0]; 359 | } __packed *resp; 360 | uint32_t resp_size = sizeof(*resp); 361 | uint32_t masks_size = MSG_RANGE_TO_SIZE(req->range); 362 | 363 | if (sizeof(*req) + masks_size != len) 364 | return -EMSGSIZE; 365 | 366 | if (diag_cmd_set_msg_mask(req->range, req->masks) == 0) { 367 | resp_size += masks_size; 368 | resp = malloc(resp_size); 369 | if (!resp) { 370 | warn("Failed to allocate response packet\n"); 371 | return -errno; 372 | } 373 | resp->header = req->header; 374 | resp->range = req->range; 375 | resp->status = DIAG_CMD_MSG_STATUS_SUCCESSFUL; 376 | resp->rsvd = req->rsvd; 377 | memcpy(resp->rt_masks, req->masks, masks_size); 378 | 379 | peripheral_broadcast_msg_mask(&resp->range); 380 | } else { 381 | resp = malloc(resp_size); 382 | if (!resp) { 383 | warn("Failed to allocate response packet\n"); 384 | return -errno; 385 | } 386 | resp->header = req->header; 387 | resp->range = req->range; 388 | resp->rsvd = req->rsvd; 389 | resp->status = DIAG_CMD_MSG_STATUS_UNSUCCESSFUL; 390 | } 391 | 392 | dm_send(client, resp, resp_size); 393 | free(resp); 394 | 395 | break; 396 | } 397 | case DIAG_CMD_OP_SET_ALL_MSG_MASK: { 398 | const struct { 399 | struct diag_msg_cmd_header header; 400 | uint8_t rsvd; 401 | uint32_t mask; 402 | } __packed *req = buf; 403 | struct { 404 | struct diag_msg_cmd_header header; 405 | uint8_t status; 406 | uint8_t rsvd; 407 | uint32_t rt_mask; 408 | } __packed resp; 409 | 410 | if (sizeof(*req) != len) 411 | return -EMSGSIZE; 412 | 413 | diag_cmd_set_all_msg_mask(req->mask); 414 | resp.header = req->header; 415 | resp.rsvd = req->rsvd; 416 | resp.rt_mask = req->mask; 417 | resp.status = DIAG_CMD_MSG_STATUS_SUCCESSFUL; 418 | 419 | peripheral_broadcast_msg_mask(NULL); 420 | 421 | dm_send(client, &resp, sizeof(resp)); 422 | break; 423 | } 424 | default: 425 | warnx("Unknown extended message configuration: %d", request_header->operation); 426 | return -EINVAL; 427 | } 428 | 429 | return 0; 430 | } 431 | 432 | static int handle_event_get_mask(struct diag_client *client, const void *buf, 433 | size_t len) 434 | { 435 | const struct { 436 | uint8_t cmd_code; 437 | uint8_t pad; 438 | uint16_t reserved; 439 | } __packed *req = buf; 440 | struct { 441 | uint8_t cmd_code; 442 | uint8_t error_code; 443 | uint16_t reserved; 444 | uint16_t num_bits; 445 | uint8_t mask[0]; 446 | } __packed *resp; 447 | uint32_t resp_size = sizeof(*resp); 448 | uint16_t num_bits = event_max_num_bits; 449 | uint16_t mask_size = 0; 450 | uint8_t *mask = NULL; 451 | 452 | if (sizeof(*req) != len) 453 | return -EMSGSIZE; 454 | 455 | if (diag_cmd_get_event_mask(num_bits, &mask) == 0) { 456 | mask_size = BITS_TO_BYTES(num_bits); 457 | resp_size += mask_size; 458 | resp = malloc(resp_size); 459 | if (!resp) { 460 | warn("Failed to allocate response packet\n"); 461 | return -errno; 462 | } 463 | resp->cmd_code = req->cmd_code; 464 | resp->reserved = req->reserved; 465 | resp->num_bits = num_bits; 466 | if (mask != NULL) { 467 | memcpy(&resp->mask, mask, mask_size); 468 | free(mask); 469 | } 470 | resp->error_code = DIAG_CMD_EVENT_ERROR_CODE_OK; 471 | } else { 472 | resp = malloc(resp_size); 473 | if (!resp) { 474 | warn("Failed to allocate response packet\n"); 475 | return -errno; 476 | } 477 | resp->cmd_code = req->cmd_code; 478 | resp->reserved = req->reserved; 479 | resp->num_bits = 0; 480 | resp->error_code = DIAG_CMD_EVENT_ERROR_CODE_FAIL; 481 | } 482 | 483 | dm_send(client, resp, resp_size); 484 | free(resp); 485 | 486 | return 0; 487 | } 488 | 489 | static int handle_event_set_mask(struct diag_client *client, 490 | const void *buf, size_t len) 491 | { 492 | const struct { 493 | uint8_t cmd_code; 494 | uint8_t pad; 495 | uint16_t reserved; 496 | uint16_t num_bits; 497 | uint8_t mask[0]; 498 | } __packed *req = buf; 499 | struct { 500 | uint8_t cmd_code; 501 | uint8_t error_code; 502 | uint16_t reserved; 503 | uint16_t num_bits; 504 | uint8_t mask[0]; 505 | } __packed *resp; 506 | uint32_t resp_size = sizeof(*resp); 507 | uint16_t mask_size = BITS_TO_BYTES(req->num_bits); 508 | 509 | if (sizeof(*req) + mask_size != len) 510 | return -EMSGSIZE; 511 | 512 | if (diag_cmd_update_event_mask(req->num_bits, req->mask) == 0) { 513 | resp_size += mask_size; 514 | resp = malloc(resp_size); 515 | if (!resp) { 516 | warn("Failed to allocate response packet\n"); 517 | return -errno; 518 | } 519 | resp->cmd_code = req->cmd_code; 520 | resp->reserved = req->reserved; 521 | resp->num_bits = req->num_bits; 522 | memcpy(resp->mask, req->mask, mask_size); 523 | resp->error_code = DIAG_CMD_EVENT_ERROR_CODE_OK; 524 | 525 | peripheral_broadcast_event_mask(); 526 | } else { 527 | resp = malloc(resp_size); 528 | if (!resp) { 529 | warn("Failed to allocate response packet\n"); 530 | return -errno; 531 | } 532 | resp->cmd_code = req->cmd_code; 533 | resp->reserved = req->reserved; 534 | resp->num_bits = 0; 535 | resp->error_code = DIAG_CMD_EVENT_ERROR_CODE_FAIL; 536 | } 537 | 538 | dm_send(client, resp, resp_size); 539 | free(resp); 540 | 541 | return 0; 542 | } 543 | 544 | static int handle_event_report_control(struct diag_client *client, 545 | const void *buf, size_t len) 546 | { 547 | const struct { 548 | uint8_t cmd_code; 549 | uint8_t operation_switch; 550 | } __packed *req = buf; 551 | struct { 552 | uint8_t cmd_code; 553 | uint16_t length; 554 | } __packed pkt; 555 | 556 | if (sizeof(*req) != len) 557 | return -EMSGSIZE; 558 | 559 | diag_cmd_toggle_events(!!req->operation_switch); 560 | peripheral_broadcast_event_mask(); 561 | 562 | pkt.cmd_code = DIAG_CMD_EVENT_REPORT_CONTROL; 563 | pkt.length = 0; 564 | 565 | dm_send(client, &pkt, sizeof(pkt)); 566 | 567 | return 0; 568 | } 569 | 570 | void register_common_cmds(void) 571 | { 572 | register_common_cmd(DIAG_CMD_LOGGING_CONFIGURATION, handle_logging_configuration); 573 | register_common_cmd(DIAG_CMD_EXTENDED_MESSAGE_CONFIGURATION, handle_extended_message_configuration); 574 | register_common_cmd(DIAG_CMD_GET_MASK, handle_event_get_mask); 575 | register_common_cmd(DIAG_CMD_SET_MASK, handle_event_set_mask); 576 | register_common_cmd(DIAG_CMD_EVENT_REPORT_CONTROL, handle_event_report_control); 577 | } 578 | -------------------------------------------------------------------------------- /router/diag.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "diag.h" 41 | #include "hdlc.h" 42 | #include "masks.h" 43 | #include "mbuf.h" 44 | #include "peripheral.h" 45 | #include "util.h" 46 | #include "watch.h" 47 | 48 | struct list_head diag_cmds = LIST_INIT(diag_cmds); 49 | 50 | void queue_push_flow(struct list_head *queue, const void *msg, size_t msglen, 51 | struct watch_flow *flow) 52 | { 53 | struct mbuf *mbuf; 54 | void *ptr; 55 | 56 | mbuf = mbuf_alloc(msglen); 57 | ptr = mbuf_put(mbuf, msglen); 58 | memcpy(ptr, msg, msglen); 59 | 60 | mbuf->flow = flow; 61 | 62 | watch_flow_inc(flow); 63 | 64 | list_add(queue, &mbuf->node); 65 | } 66 | 67 | void queue_push(struct list_head *queue, const void *msg, size_t msglen) 68 | { 69 | queue_push_flow(queue, msg, msglen, NULL); 70 | } 71 | 72 | static void usage(void) 73 | { 74 | fprintf(stderr, 75 | "User space application for diag interface\n" 76 | "\n" 77 | "usage: diag [-hsu]\n" 78 | "\n" 79 | "options:\n" 80 | " -h show this usage\n" 81 | " -s \n" 82 | " -u \n" 83 | ); 84 | 85 | exit(1); 86 | } 87 | 88 | int main(int argc, char **argv) 89 | { 90 | char *host_address = NULL; 91 | int host_port = DEFAULT_SOCKET_PORT; 92 | char *uartdev = NULL; 93 | int baudrate = DEFAULT_BAUD_RATE; 94 | char *token; 95 | int ret; 96 | int c; 97 | 98 | for (;;) { 99 | c = getopt(argc, argv, "hs:u:"); 100 | if (c < 0) 101 | break; 102 | switch (c) { 103 | case 's': 104 | host_address = strtok(strdup(optarg), ":"); 105 | token = strtok(NULL, ""); 106 | if (token) 107 | host_port = atoi(token); 108 | break; 109 | case 'u': 110 | uartdev = strtok(strdup(optarg), "@"); 111 | token = strtok(NULL, ""); 112 | if (token) 113 | baudrate = atoi(token); 114 | break; 115 | default: 116 | case 'h': 117 | usage(); 118 | break; 119 | } 120 | } 121 | 122 | if (host_address) { 123 | ret = diag_sock_connect(host_address, host_port); 124 | if (ret < 0) 125 | err(1, "failed to connect to client"); 126 | } else if (uartdev) { 127 | ret = diag_uart_open(uartdev, baudrate); 128 | if (ret < 0) 129 | errx(1, "failed to open uart\n"); 130 | } 131 | 132 | diag_usb_open("/dev/ffs-diag"); 133 | 134 | ret = diag_unix_open(); 135 | if (ret < 0) 136 | errx(1, "failed to create unix socket dm\n"); 137 | 138 | peripheral_init(); 139 | 140 | diag_masks_init(); 141 | 142 | register_app_cmds(); 143 | register_common_cmds(); 144 | 145 | watch_run(); 146 | 147 | return 0; 148 | } 149 | -------------------------------------------------------------------------------- /router/diag.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #ifndef __DIAG_H__ 33 | #define __DIAG_H__ 34 | 35 | #include 36 | #include 37 | 38 | #include "circ_buf.h" 39 | #include "hdlc.h" 40 | #include "list.h" 41 | #include "watch.h" 42 | 43 | #define DEFAULT_SOCKET_PORT 2500 44 | #define DEFAULT_BAUD_RATE 115200 45 | 46 | #define BIT(x) (1 << (x)) 47 | 48 | #define DIAG_FEATURE_FEATURE_MASK_SUPPORT BIT(0) 49 | #define DIAG_FEATURE_DIAG_MASTER_SETS_COMMON_MASK BIT(1) 50 | #define DIAG_FEATURE_LOG_ON_DEMAND_APPS BIT(2) 51 | #define DIAG_FEATURE_DIAG_VERSION_RSP_ON_MASTER BIT(3) 52 | #define DIAG_FEATURE_REQ_RSP_SUPPORT BIT(4) 53 | #define DIAG_FEATURE_DIAG_PRESET_MASKS BIT(5) 54 | #define DIAG_FEATURE_APPS_HDLC_ENCODE BIT(6) 55 | #define DIAG_FEATURE_STM BIT(9) 56 | #define DIAG_FEATURE_PERIPHERAL_BUFFERING BIT(10) 57 | #define DIAG_FEATURE_MASK_CENTRALIZATION BIT(11) 58 | #define DIAG_FEATURE_SOCKETS_ENABLED BIT(13) 59 | #define DIAG_FEATURE_DCI_EXTENDED_HEADER BIT(14) 60 | #define DIAG_FEATURE_DIAG_ID BIT(15) 61 | #define DIAG_FEATURE_PKT_HEADER_UNTAG BIT(16) 62 | #define DIAG_FEATURE_DIAG_ID_FEATURE_MASK BIT(19) 63 | 64 | #define DIAG_CMD_SUBSYS_DISPATCH 75 65 | #define DIAG_CMD_SUBSYS_DISPATCH_V2 128 66 | 67 | struct diag_client; 68 | 69 | struct peripheral { 70 | struct list_head node; 71 | 72 | char *name; 73 | 74 | unsigned long features; 75 | 76 | struct list_head cmdq; 77 | struct list_head cntlq; 78 | struct list_head dataq; 79 | 80 | int cntl_fd; 81 | bool cntl_open; 82 | int data_fd; 83 | bool data_open; 84 | int cmd_fd; 85 | int dci_cmd_fd; 86 | 87 | struct watch_flow *flow; 88 | 89 | int diag_id; 90 | 91 | bool sockets; 92 | 93 | struct circ_buf recv_buf; 94 | struct hdlc_decoder recv_decoder; 95 | 96 | int (*send)(struct peripheral *perif, const void *ptr, size_t len); 97 | void (*close)(struct peripheral *perif); 98 | }; 99 | 100 | extern struct list_head peripherals; 101 | 102 | struct diag_cmd { 103 | struct list_head node; 104 | 105 | unsigned int first; 106 | unsigned int last; 107 | 108 | struct peripheral *peripheral; 109 | int(*cb)(struct diag_client *client, const void *buf, size_t len); 110 | }; 111 | 112 | void queue_push(struct list_head *queue, const void *msg, size_t msglen); 113 | void queue_push_flow(struct list_head *queue, const void *msg, size_t msglen, 114 | struct watch_flow *flow); 115 | 116 | extern struct list_head diag_cmds; 117 | 118 | int diag_sock_connect(const char *hostname, unsigned short port); 119 | int diag_uart_open(const char *uartname, unsigned int baudrate); 120 | int diag_usb_open(const char *ffs_name); 121 | int diag_unix_open(void); 122 | 123 | int diag_client_handle_command(struct diag_client *client, uint8_t *data, size_t len); 124 | 125 | int hdlc_enqueue(struct list_head *queue, const void *buf, size_t msglen); 126 | int hdlc_enqueue_flow(struct list_head *queue, const void *buf, size_t msglen, 127 | struct watch_flow *flow); 128 | 129 | void register_fallback_cmd(unsigned int cmd, 130 | int(*cb)(struct diag_client *client, 131 | const void *buf, size_t len)); 132 | void register_fallback_subsys_cmd(unsigned int subsys, unsigned int cmd, 133 | int(*cb)(struct diag_client *client, 134 | const void *buf, size_t len)); 135 | void register_common_cmd(unsigned int cmd, int(*cb)(struct diag_client *client, 136 | const void *buf, 137 | size_t len)); 138 | 139 | void register_app_cmds(void); 140 | void register_common_cmds(void); 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /router/diag_cntl.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "diag.h" 41 | #include "diag_cntl.h" 42 | #include "masks.h" 43 | #include "peripheral.h" 44 | #include "util.h" 45 | 46 | #define DIAG_CTRL_MSG_DTR 2 47 | #define DIAG_CTRL_MSG_DIAGMODE 3 48 | #define DIAG_CTRL_MSG_DIAGDATA 4 49 | #define DIAG_CTRL_MSG_FEATURE 8 50 | #define DIAG_CTRL_MSG_EQUIP_LOG_MASK 9 51 | #define DIAG_CTRL_MSG_EVENT_MASK_V2 10 52 | #define DIAG_CTRL_MSG_F3_MASK_V2 11 53 | #define DIAG_CTRL_MSG_NUM_PRESETS 12 54 | #define DIAG_CTRL_MSG_SET_PRESET_ID 13 55 | #define DIAG_CTRL_MSG_LOG_MASK_WITH_PRESET_ID 14 56 | #define DIAG_CTRL_MSG_EVENT_MASK_WITH_PRESET_ID 15 57 | #define DIAG_CTRL_MSG_F3_MASK_WITH_PRESET_ID 16 58 | #define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE 17 59 | #define DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM 18 60 | #define DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL 19 61 | #define DIAG_CTRL_MSG_DCI_CONNECTION_STATUS 20 62 | #define DIAG_CTRL_MSG_LAST_EVENT_REPORT 22 63 | #define DIAG_CTRL_MSG_LOG_RANGE_REPORT 23 64 | #define DIAG_CTRL_MSG_SSID_RANGE_REPORT 24 65 | #define DIAG_CTRL_MSG_BUILD_MASK_REPORT 25 66 | #define DIAG_CTRL_MSG_DEREG 27 67 | #define DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT 29 68 | #define DIAG_CTRL_MSG_PD_STATUS 30 69 | #define DIAG_CTRL_MSG_TIME_SYNC_PKT 31 70 | 71 | struct diag_cntl_hdr { 72 | uint32_t cmd; 73 | uint32_t len; 74 | }; 75 | 76 | struct cmd_range_reg { 77 | uint16_t first; 78 | uint16_t last; 79 | uint32_t data; 80 | }; 81 | 82 | #define DIAG_CNTL_CMD_REGISTER 1 83 | struct diag_cntl_cmd_reg { 84 | struct diag_cntl_hdr hdr; 85 | uint32_t version; 86 | uint16_t cmd; 87 | uint16_t subsys; 88 | uint16_t count_entries; 89 | uint16_t port; 90 | struct cmd_range_reg ranges[]; 91 | } __packed; 92 | #define to_cmd_reg(h) container_of(h, struct diag_cntl_cmd_reg, hdr) 93 | 94 | #define DIAG_CNTL_CMD_DIAG_MODE 3 95 | struct diag_cntl_cmd_diag_mode 96 | { 97 | struct diag_cntl_hdr hdr; 98 | uint32_t version; 99 | uint32_t sleep_vote; 100 | uint32_t real_time; 101 | uint32_t use_nrt_values; 102 | uint32_t commit_threshold; 103 | uint32_t sleep_threshold; 104 | uint32_t sleep_time; 105 | uint32_t drain_timer_val; 106 | uint32_t event_stale_time_val; 107 | } __packed; 108 | 109 | struct diag_cntl_cmd_diag_mode_v2 110 | { 111 | struct diag_cntl_hdr hdr; 112 | uint32_t version; 113 | uint32_t sleep_vote; 114 | uint32_t real_time; 115 | uint32_t use_nrt_values; 116 | uint32_t commit_threshold; 117 | uint32_t sleep_threshold; 118 | uint32_t sleep_time; 119 | uint32_t drain_timer_val; 120 | uint32_t event_stale_time_val; 121 | uint8_t diag_id; 122 | } __packed; 123 | 124 | #define DIAG_CNTL_CMD_FEATURE_MASK 8 125 | struct diag_cntl_cmd_feature { 126 | struct diag_cntl_hdr hdr; 127 | uint32_t mask_len; 128 | uint32_t mask; 129 | } __packed; 130 | #define to_cmd_feature(h) container_of(h, struct diag_cntl_cmd_feature, hdr) 131 | 132 | #define DIAG_CNTL_CMD_LOG_MASK 9 133 | struct diag_cntl_cmd_log_mask { 134 | struct diag_cntl_hdr hdr; 135 | uint8_t stream_id; 136 | uint8_t status; 137 | uint8_t equip_id; 138 | uint32_t last_item; 139 | uint32_t log_mask_size; 140 | uint8_t equip_log_mask[]; 141 | } __packed; 142 | 143 | #define DIAG_CNTL_CMD_MSG_MASK 11 144 | struct diag_cntl_cmd_msg_mask { 145 | struct diag_cntl_hdr hdr; 146 | uint8_t stream_id; 147 | uint8_t status; 148 | uint8_t msg_mode; 149 | struct diag_ssid_range_t range; 150 | uint32_t msg_mask_len; 151 | uint8_t range_msg_mask[]; 152 | } __packed; 153 | 154 | #define DIAG_CNTL_CMD_EVENT_MASK 10 155 | struct diag_cntl_cmd_event_mask { 156 | struct diag_cntl_hdr hdr; 157 | uint8_t stream_id; 158 | uint8_t status; 159 | uint8_t event_config; 160 | uint32_t event_mask_len; 161 | uint8_t event_mask[]; 162 | } __packed; 163 | 164 | #define DIAG_CNTL_CMD_NUM_PRESETS 12 165 | struct diag_cntl_num_presets { 166 | struct diag_cntl_hdr hdr; 167 | uint8_t num; 168 | }; 169 | 170 | struct cmd_range_dereg { 171 | uint16_t first; 172 | uint16_t last; 173 | }; 174 | 175 | #define DIAG_CNTL_CMD_BUFFERING_TX_MODE 17 176 | struct diag_cntl_cmd_buffering_tx_mode 177 | { 178 | struct diag_cntl_hdr hdr; 179 | uint32_t version; 180 | uint8_t stream_id; 181 | uint8_t tx_mode; 182 | } __packed; 183 | 184 | struct diag_cntl_cmd_buffering_tx_mode_v2 185 | { 186 | struct diag_cntl_hdr hdr; 187 | uint32_t version; 188 | uint8_t diag_id; 189 | uint8_t stream_id; 190 | uint8_t tx_mode; 191 | } __packed; 192 | 193 | #define DIAG_BUFFERING_MODE_STREAMING 0 194 | #define DIAG_BUFFERING_MODE_THRESHOLD 1 195 | #define DIAG_BUFFERING_MODE_CIRCULAR 2 196 | 197 | #define DIAG_CNTL_CMD_DEREGISTER 27 198 | struct diag_cntl_cmd_dereg { 199 | struct diag_cntl_hdr hdr; 200 | uint32_t version; 201 | uint16_t cmd; 202 | uint16_t subsys; 203 | uint16_t count_entries; 204 | struct cmd_range_dereg ranges[]; 205 | } __packed; 206 | #define to_cmd_dereg(h) container_of(h, struct diag_cntl_cmd_dereg, hdr) 207 | 208 | #define DIAG_CNTL_CMD_DIAG_ID 33 209 | struct diag_cntl_cmd_diag_id { 210 | struct diag_cntl_hdr hdr; 211 | uint32_t version; 212 | uint32_t diag_id; 213 | char process_name[]; 214 | } __packed; 215 | #define to_cmd_diagid(h) container_of(h, struct diag_cntl_cmd_diag_id, hdr) 216 | 217 | struct diag_cntl_cmd_diag_id_v2 { 218 | struct diag_cntl_hdr hdr; 219 | uint32_t version; 220 | uint32_t diag_id; 221 | uint32_t feature_len; 222 | uint8_t *feature_mask; 223 | char process_name[]; 224 | } __packed; 225 | #define to_cmd_diag_id_v2(h) container_of(h, struct diag_cntl_cmd_diag_id_v2, hdr) 226 | 227 | struct list_head diag_ids = LIST_INIT(diag_ids); 228 | 229 | static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask); 230 | 231 | static int diag_cntl_register(struct peripheral *peripheral, 232 | struct diag_cntl_hdr *hdr, size_t len) 233 | { 234 | struct diag_cntl_cmd_reg *pkt = to_cmd_reg(hdr); 235 | struct diag_cmd *dc; 236 | unsigned int subsys; 237 | unsigned int cmd; 238 | unsigned int first; 239 | unsigned int last; 240 | int i; 241 | 242 | for (i = 0; i < pkt->count_entries; i++) { 243 | cmd = pkt->cmd; 244 | subsys = pkt->subsys; 245 | 246 | if (cmd == 0xff && subsys != 0xff) 247 | cmd = DIAG_CMD_SUBSYS_DISPATCH; 248 | 249 | first = cmd << 24 | subsys << 16 | pkt->ranges[i].first; 250 | last = cmd << 24 | subsys << 16 | pkt->ranges[i].last; 251 | 252 | // printf("[%s] register 0x%x - 0x%x\n", 253 | // peripheral->name, first, last); 254 | 255 | dc = malloc(sizeof(*dc)); 256 | if (!dc) { 257 | warn("malloc failed"); 258 | return -ENOMEM; 259 | } 260 | memset(dc, 0, sizeof(*dc)); 261 | 262 | dc->first = first; 263 | dc->last = last; 264 | dc->peripheral = peripheral; 265 | 266 | list_add(&diag_cmds, &dc->node); 267 | } 268 | 269 | return 0; 270 | } 271 | 272 | static int diag_cntl_feature_mask(struct peripheral *peripheral, 273 | struct diag_cntl_hdr *hdr, size_t len) 274 | { 275 | struct diag_cntl_cmd_feature *pkt = to_cmd_feature(hdr); 276 | uint32_t local_mask = 0; 277 | uint32_t mask = pkt->mask; 278 | 279 | local_mask |= DIAG_FEATURE_FEATURE_MASK_SUPPORT; 280 | local_mask |= DIAG_FEATURE_DIAG_MASTER_SETS_COMMON_MASK; 281 | if (peripheral->cmd_fd >= 0) 282 | local_mask |= DIAG_FEATURE_REQ_RSP_SUPPORT; 283 | local_mask |= DIAG_FEATURE_APPS_HDLC_ENCODE; 284 | if (peripheral->sockets) 285 | local_mask |= DIAG_FEATURE_SOCKETS_ENABLED; 286 | local_mask |= DIAG_FEATURE_DIAG_ID; 287 | local_mask |= DIAG_FEATURE_DIAG_ID_FEATURE_MASK; 288 | 289 | printf("[%s] mask:", peripheral->name); 290 | 291 | if (mask & DIAG_FEATURE_FEATURE_MASK_SUPPORT) 292 | printf(" FEATURE_MASK_SUPPORT"); 293 | if (mask & DIAG_FEATURE_DIAG_MASTER_SETS_COMMON_MASK) 294 | printf(" DIAG_MASTER_SETS_COMMON_MASK"); 295 | if (mask & DIAG_FEATURE_LOG_ON_DEMAND_APPS) 296 | printf(" LOG_ON_DEMAND"); 297 | if (mask & DIAG_FEATURE_DIAG_VERSION_RSP_ON_MASTER) 298 | printf(" DIAG_VERSION_RSP_ON_MASTER"); 299 | if (mask & DIAG_FEATURE_REQ_RSP_SUPPORT) 300 | printf(" REQ_RSP"); 301 | if (mask & DIAG_FEATURE_APPS_HDLC_ENCODE) 302 | printf(" APPS_HDLC_ENCODE"); 303 | if (mask & DIAG_FEATURE_STM) 304 | printf(" STM"); 305 | if (mask & DIAG_FEATURE_PERIPHERAL_BUFFERING) 306 | printf(" PERIPHERAL-BUFFERING"); 307 | if (mask & DIAG_FEATURE_MASK_CENTRALIZATION) 308 | printf(" MASK-CENTERALIZATION"); 309 | if (mask & DIAG_FEATURE_SOCKETS_ENABLED) 310 | printf(" SOCKETS"); 311 | if (mask & DIAG_FEATURE_DIAG_ID) 312 | printf(" DIAG-ID"); 313 | if (mask & DIAG_FEATURE_DIAG_ID_FEATURE_MASK) { 314 | printf(" DIAG-ID-FEATURE-MASK"); 315 | } 316 | 317 | printf(" (0x%x)\n", mask); 318 | 319 | peripheral->features = mask & local_mask; 320 | 321 | diag_cntl_send_feature_mask(peripheral, peripheral->features); 322 | 323 | return 0; 324 | } 325 | 326 | void diag_cntl_send_log_mask(struct peripheral *peripheral, uint32_t equip_id) 327 | { 328 | struct diag_cntl_cmd_log_mask *pkt; 329 | size_t len = sizeof(*pkt); 330 | uint32_t num_items = 0; 331 | uint8_t *mask = NULL; 332 | uint32_t mask_size = 0; 333 | uint8_t status = diag_get_log_mask_status(); 334 | 335 | if (peripheral == NULL) 336 | return; 337 | if (peripheral->cntl_fd == -1) { 338 | warn("Peripheral %s has no control channel. Skipping!\n", peripheral->name); 339 | return; 340 | } 341 | 342 | if (status == DIAG_CTRL_MASK_VALID) { 343 | diag_cmd_get_log_mask(equip_id, &num_items, &mask, &mask_size); 344 | } else { 345 | equip_id = 0; 346 | } 347 | len += mask_size; 348 | 349 | pkt = alloca(len); 350 | 351 | pkt->hdr.cmd = DIAG_CNTL_CMD_LOG_MASK; 352 | pkt->hdr.len = len - sizeof(struct diag_cntl_hdr); 353 | pkt->stream_id = 1; 354 | pkt->status = status; 355 | pkt->equip_id = equip_id; 356 | pkt->last_item = num_items; 357 | pkt->log_mask_size = mask_size; 358 | if (mask != NULL) { 359 | memcpy(pkt->equip_log_mask, mask, mask_size); 360 | free(mask); 361 | } 362 | 363 | queue_push(&peripheral->cntlq, pkt, len); 364 | } 365 | 366 | void diag_cntl_send_msg_mask(struct peripheral *peripheral, struct diag_ssid_range_t *range) 367 | { 368 | struct diag_cntl_cmd_msg_mask *pkt; 369 | size_t len = sizeof(*pkt); 370 | uint32_t num_items = 0; 371 | uint32_t *mask = NULL; 372 | uint32_t mask_size = 0; 373 | struct diag_ssid_range_t DUMMY_RANGE = { 0, 0 }; 374 | uint8_t status = diag_get_msg_mask_status(); 375 | 376 | if (peripheral == NULL) 377 | return; 378 | if (peripheral->cntl_fd == -1) { 379 | warn("Peripheral %s has no control channel. Skipping!\n", peripheral->name); 380 | return; 381 | } 382 | 383 | if (status == DIAG_CTRL_MASK_VALID) { 384 | diag_cmd_get_msg_mask(range, &mask); 385 | num_items = range->ssid_last - range->ssid_first + 1; 386 | } else if (status == DIAG_CTRL_MASK_ALL_DISABLED) { 387 | range = &DUMMY_RANGE; 388 | num_items = 0; 389 | } else if (status == DIAG_CTRL_MASK_ALL_ENABLED) { 390 | diag_cmd_get_msg_mask(range, &mask); 391 | num_items = 1; 392 | } 393 | mask_size = num_items * sizeof(*mask); 394 | len += mask_size; 395 | 396 | pkt = alloca(len); 397 | 398 | pkt->hdr.cmd = DIAG_CNTL_CMD_MSG_MASK; 399 | pkt->hdr.len = len - sizeof(struct diag_cntl_hdr); 400 | pkt->stream_id = 1; 401 | pkt->status = status; 402 | pkt->msg_mode = 0; 403 | pkt->range = *range; 404 | pkt->msg_mask_len = num_items; 405 | if (mask != NULL) { 406 | memcpy(pkt->range_msg_mask, mask, mask_size); 407 | free(mask); 408 | } 409 | 410 | queue_push(&peripheral->cntlq, pkt, len); 411 | } 412 | 413 | void diag_cntl_send_masks(struct peripheral *peripheral) 414 | { 415 | struct diag_ssid_range_t range; 416 | int i; 417 | 418 | for (i = 0; i < MSG_MASK_TBL_CNT; i++) { 419 | range.ssid_first = ssid_first_arr[i]; 420 | range.ssid_last = ssid_last_arr[i]; 421 | 422 | diag_cntl_send_msg_mask(peripheral, &range); 423 | } 424 | } 425 | 426 | void diag_cntl_send_event_mask(struct peripheral *peripheral) 427 | { 428 | struct diag_cntl_cmd_event_mask *pkt; 429 | size_t len = sizeof(*pkt); 430 | uint8_t *mask = NULL; 431 | uint16_t mask_size = 0; 432 | uint8_t status = diag_get_event_mask_status(); 433 | uint8_t event_config = (status == DIAG_CTRL_MASK_ALL_ENABLED || status == DIAG_CTRL_MASK_VALID) ? 0x1 : 0x0; 434 | 435 | if (peripheral == NULL) 436 | return; 437 | if (peripheral->cntl_fd == -1) { 438 | warn("Peripheral %s has no control channel. Skipping!\n", peripheral->name); 439 | return; 440 | } 441 | 442 | if (status == DIAG_CTRL_MASK_VALID) { 443 | if (diag_cmd_get_event_mask(event_max_num_bits , &mask) == 0) { 444 | mask_size = BITS_TO_BYTES(event_max_num_bits); 445 | } 446 | } 447 | len += mask_size; 448 | 449 | pkt = alloca(len); 450 | 451 | pkt->hdr.cmd = DIAG_CNTL_CMD_EVENT_MASK; 452 | pkt->hdr.len = len - sizeof(struct diag_cntl_hdr); 453 | pkt->stream_id = 1; 454 | pkt->status = status; 455 | pkt->event_config = event_config; 456 | pkt->event_mask_len = mask_size; 457 | if (mask != NULL) { 458 | memcpy(pkt->event_mask, mask, mask_size); 459 | free(mask); 460 | } 461 | 462 | queue_push(&peripheral->cntlq, pkt, len); 463 | } 464 | 465 | static int diag_cntl_deregister(struct peripheral *peripheral, 466 | struct diag_cntl_hdr *hdr, size_t len) 467 | { 468 | struct diag_cntl_cmd_dereg *pkt = to_cmd_dereg(hdr); 469 | struct diag_cmd *dc; 470 | unsigned int subsys; 471 | unsigned int cmd; 472 | unsigned int first; 473 | unsigned int last; 474 | int i; 475 | struct list_head *item; 476 | struct list_head *next; 477 | 478 | for (i = 0; i < pkt->count_entries; i++) { 479 | cmd = pkt->cmd; 480 | subsys = pkt->subsys; 481 | 482 | if (cmd == 0xff && subsys != 0xff) 483 | cmd = DIAG_CMD_SUBSYS_DISPATCH; 484 | 485 | first = cmd << 24 | subsys << 16 | pkt->ranges[i].first; 486 | last = cmd << 24 | subsys << 16 | pkt->ranges[i].last; 487 | 488 | list_for_each_safe(item, next, &diag_cmds) { 489 | dc = container_of(item, struct diag_cmd, node); 490 | if (dc->peripheral == peripheral && dc->first == first && dc->last == last) { 491 | list_del(&dc->node); 492 | } 493 | } 494 | } 495 | 496 | return 0; 497 | } 498 | 499 | static void diag_cntl_send_feature_mask(struct peripheral *peripheral, uint32_t mask) 500 | { 501 | struct diag_cntl_cmd_feature *pkt; 502 | size_t len = sizeof(*pkt) + 2; 503 | 504 | if (peripheral->cntl_fd == -1) { 505 | warn("Peripheral %s has no control channel. Skipping!\n", peripheral->name); 506 | return; 507 | } 508 | 509 | pkt = alloca(len); 510 | 511 | pkt->hdr.cmd = DIAG_CNTL_CMD_FEATURE_MASK; 512 | pkt->hdr.len = len - sizeof(struct diag_cntl_hdr); 513 | pkt->mask_len = sizeof(pkt->mask); 514 | pkt->mask = mask; 515 | 516 | queue_push(&peripheral->cntlq, pkt, len); 517 | /*send other control packets after sending feature mask */ 518 | diag_cntl_send_masks(peripheral); 519 | diag_cntl_set_diag_mode(peripheral, true); 520 | diag_cntl_set_buffering_mode(peripheral, 0); 521 | } 522 | 523 | void diag_cntl_set_diag_mode(struct peripheral *perif, bool real_time) 524 | { 525 | struct diag_cntl_cmd_diag_mode_v2 pkt_v2; 526 | struct diag_cntl_cmd_diag_mode pkt; 527 | 528 | if (perif->diag_id) { 529 | pkt_v2.hdr.cmd = DIAG_CNTL_CMD_DIAG_MODE; 530 | pkt_v2.hdr.len = 37; 531 | pkt_v2.version = 2; 532 | pkt_v2.sleep_vote = real_time; 533 | pkt_v2.real_time = real_time; 534 | pkt_v2.use_nrt_values = 0; 535 | pkt_v2.commit_threshold = 0; 536 | pkt_v2.sleep_threshold = 0; 537 | pkt_v2.sleep_time = 0; 538 | pkt_v2.drain_timer_val = 0; 539 | pkt_v2.event_stale_time_val = 0; 540 | pkt_v2.diag_id = perif->diag_id; 541 | 542 | queue_push(&perif->cntlq, &pkt_v2, sizeof(pkt_v2)); 543 | } else { 544 | pkt.hdr.cmd = DIAG_CNTL_CMD_DIAG_MODE; 545 | pkt.hdr.len = 36; 546 | pkt.version = 1; 547 | pkt.sleep_vote = real_time; 548 | pkt.real_time = real_time; 549 | pkt.use_nrt_values = 0; 550 | pkt.commit_threshold = 0; 551 | pkt.sleep_threshold = 0; 552 | pkt.sleep_time = 0; 553 | pkt.drain_timer_val = 0; 554 | pkt.event_stale_time_val = 0; 555 | 556 | queue_push(&perif->cntlq, &pkt, sizeof(pkt)); 557 | } 558 | } 559 | 560 | void diag_cntl_set_buffering_mode(struct peripheral *perif, int mode) 561 | { 562 | struct diag_cntl_cmd_buffering_tx_mode_v2 pkt_v2; 563 | struct diag_cntl_cmd_buffering_tx_mode pkt; 564 | 565 | if (perif->diag_id) { 566 | pkt_v2.hdr.cmd = DIAG_CNTL_CMD_BUFFERING_TX_MODE; 567 | pkt_v2.hdr.len = 7; 568 | pkt_v2.version = 2; 569 | pkt_v2.diag_id = perif->diag_id; 570 | pkt_v2.stream_id = 0; 571 | pkt_v2.tx_mode = mode; 572 | 573 | queue_push(&perif->cntlq, &pkt_v2, sizeof(pkt_v2)); 574 | } else { 575 | pkt.hdr.cmd = DIAG_CNTL_CMD_BUFFERING_TX_MODE; 576 | pkt.hdr.len = 6; 577 | pkt.version = 1; 578 | pkt.stream_id = 0; 579 | pkt.tx_mode = mode; 580 | 581 | queue_push(&perif->cntlq, &pkt, sizeof(pkt)); 582 | } 583 | } 584 | 585 | struct list_head *diag_get_diag_ids_head(void) 586 | { 587 | return &diag_ids; 588 | } 589 | 590 | int register_diag_id(uint8_t diag_id, const char *process_name, uint8_t len) 591 | { 592 | struct diag_id_tbl_t *new_diag_id = NULL; 593 | 594 | if (!process_name || !len || !diag_id) 595 | return -EINVAL; 596 | 597 | new_diag_id = malloc(sizeof(struct diag_id_tbl_t) + len); 598 | if (!new_diag_id) 599 | return -ENOMEM; 600 | 601 | new_diag_id->diag_id_info_len = sizeof(struct diag_id_info) + len; 602 | new_diag_id->diagid_info.diag_id = diag_id; 603 | new_diag_id->diagid_info.process_name_len = len; 604 | strncpy(new_diag_id->diagid_info.process_name, process_name, len - 1); 605 | new_diag_id->diagid_info.process_name[len - 1] = '\0'; 606 | list_add(&diag_ids, &new_diag_id->node); 607 | 608 | return 0; 609 | } 610 | 611 | int find_diag_id(const char *name, uint32_t *diag_id) 612 | { 613 | struct diag_id_tbl_t *diag_id_item = NULL; 614 | 615 | if (!name || !diag_id) 616 | return -EINVAL; 617 | 618 | list_for_each_entry(diag_id_item, &diag_ids, node) { 619 | if (!strcmp(diag_id_item->diagid_info.process_name, name)) { 620 | *diag_id = diag_id_item->diagid_info.diag_id; 621 | return 1; 622 | } 623 | } 624 | 625 | return 0; 626 | } 627 | 628 | bool diag_id_exists(uint32_t diag_id) 629 | { 630 | struct diag_id_tbl_t *diag_id_item = NULL; 631 | 632 | list_for_each_entry(diag_id_item, &diag_ids, node) { 633 | if (diag_id == diag_id_item->diagid_info.diag_id) 634 | return true; 635 | } 636 | 637 | return false; 638 | } 639 | 640 | static int diag_cntl_process_diag_id(struct peripheral *peripheral, struct diag_cntl_hdr *hdr, size_t len) 641 | { 642 | struct diag_cntl_cmd_diag_id_v2 *pkt_v2 = to_cmd_diag_id_v2(hdr); 643 | struct diag_cntl_cmd_diag_id *pkt = to_cmd_diagid(hdr); 644 | uint32_t version = 0, local_diag_id = 0; 645 | static uint32_t diag_id = DIAG_ID_APPS; 646 | uint8_t resp_buffer[DIAG_MAX_RSP_SIZE] = {0}; 647 | uint8_t process_name_len = 0; 648 | char *process_name = NULL; 649 | int ret; 650 | 651 | version = pkt->version; 652 | if ((peripheral->features & DIAG_FEATURE_DIAG_ID_FEATURE_MASK) && (version >= DIAG_ID_VERSION_2)) { 653 | if (len < sizeof(struct diag_cntl_cmd_diag_id_v2)) 654 | return -EINVAL; 655 | process_name = (char *)&pkt_v2->feature_mask + pkt_v2->feature_len; 656 | } else { 657 | if (len < sizeof(struct diag_cntl_cmd_diag_id)) 658 | return -EINVAL; 659 | process_name = (char*)&pkt->process_name; 660 | } 661 | 662 | process_name_len = strlen(process_name) + 1; 663 | ret = find_diag_id(process_name, &local_diag_id); 664 | if (!ret) { 665 | if (version >= DIAG_ID_VERSION_3) 666 | local_diag_id = pkt_v2->diag_id; 667 | else 668 | local_diag_id = ++diag_id; 669 | 670 | if (diag_id_exists(local_diag_id)) 671 | return -EINVAL; 672 | 673 | if (register_diag_id(local_diag_id, process_name, process_name_len)) 674 | return -EINVAL; 675 | } 676 | diag_id = local_diag_id; 677 | 678 | struct diag_cntl_cmd_diag_id *resp = (struct diag_cntl_cmd_diag_id *)resp_buffer; 679 | resp->diag_id = diag_id; 680 | resp->hdr.cmd = DIAG_CNTL_CMD_DIAG_ID; 681 | resp->version = DIAG_ID_VERSION_1; 682 | strncpy(resp->process_name, process_name, process_name_len - 1); 683 | resp->process_name[process_name_len - 1] = '\0'; 684 | resp->hdr.len = sizeof(resp->diag_id) + sizeof(resp->version) + process_name_len; 685 | len = resp->hdr.len + sizeof(resp->hdr); 686 | 687 | queue_push(&peripheral->cntlq, resp, len); 688 | return 0; 689 | } 690 | 691 | int diag_cntl_recv(struct peripheral *peripheral, const void *buf, size_t n) 692 | { 693 | struct diag_cntl_hdr *hdr; 694 | size_t offset = 0; 695 | 696 | for (;;) { 697 | if (offset + sizeof(struct diag_cntl_hdr) > n) 698 | break; 699 | 700 | hdr = (struct diag_cntl_hdr *)(buf + offset); 701 | if (offset + sizeof(struct diag_cntl_hdr) + hdr->len > n) { 702 | warnx("truncated diag cntl command"); 703 | break; 704 | } 705 | 706 | switch (hdr->cmd) { 707 | case DIAG_CNTL_CMD_REGISTER: 708 | diag_cntl_register(peripheral, hdr, n); 709 | break; 710 | case DIAG_CNTL_CMD_FEATURE_MASK: 711 | diag_cntl_feature_mask(peripheral, hdr, n); 712 | break; 713 | case DIAG_CNTL_CMD_DIAG_ID: 714 | diag_cntl_process_diag_id(peripheral, hdr, n); 715 | break; 716 | case DIAG_CNTL_CMD_NUM_PRESETS: 717 | break; 718 | case DIAG_CNTL_CMD_DEREGISTER: 719 | diag_cntl_deregister(peripheral, hdr, n); 720 | break; 721 | default: 722 | warnx("[%s] unsupported control packet: %d", 723 | peripheral->name, hdr->cmd); 724 | print_hex_dump("CNTL", buf, n); 725 | break; 726 | } 727 | 728 | offset += sizeof(struct diag_cntl_hdr) + hdr->len; 729 | } 730 | 731 | return 0; 732 | } 733 | 734 | void diag_cntl_close(struct peripheral *peripheral) 735 | { 736 | struct list_head *item; 737 | struct list_head *next; 738 | struct diag_cmd *dc; 739 | 740 | list_for_each_safe(item, next, &diag_cmds) { 741 | dc = container_of(item, struct diag_cmd, node); 742 | if (dc->peripheral == peripheral) 743 | list_del(&dc->node); 744 | } 745 | } 746 | -------------------------------------------------------------------------------- /router/diag_cntl.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #ifndef __DIAG_CNTL_H__ 33 | #define __DIAG_CNTL_H__ 34 | 35 | #include "peripheral.h" 36 | #include "masks.h" 37 | 38 | #define DIAG_ID_VERSION_1 1 39 | #define DIAG_ID_VERSION_2 2 40 | #define DIAG_ID_VERSION_3 3 41 | #define DIAG_ID_APPS 1 42 | 43 | #define DIAG_MAX_REQ_SIZE (16 * 1024) 44 | #define DIAG_MAX_RSP_SIZE (16 * 1024) 45 | 46 | struct diag_id_info 47 | { 48 | uint8_t diag_id; 49 | uint8_t process_name_len; 50 | char process_name[]; 51 | }; 52 | 53 | struct diag_id_tbl_t { 54 | struct list_head node; 55 | uint8_t diag_id_info_len; 56 | struct diag_id_info diagid_info; 57 | }; 58 | 59 | int diag_cntl_recv(struct peripheral *perif, const void *buf, size_t len); 60 | void diag_cntl_send_log_mask(struct peripheral *peripheral, uint32_t equip_id); 61 | void diag_cntl_send_msg_mask(struct peripheral *peripheral, struct diag_ssid_range_t *range); 62 | void diag_cntl_send_event_mask(struct peripheral *peripheral); 63 | void diag_cntl_close(struct peripheral *peripheral); 64 | 65 | void diag_cntl_send_masks(struct peripheral *peripheral); 66 | 67 | void diag_cntl_set_diag_mode(struct peripheral *perif, bool real_time); 68 | void diag_cntl_set_buffering_mode(struct peripheral *perif, int mode); 69 | 70 | struct list_head *diag_get_diag_ids_head(void); 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /router/dm.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016-2018, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "diag.h" 40 | #include "dm.h" 41 | #include "watch.h" 42 | 43 | /** 44 | * DOC: Diagnostic Monitor 45 | */ 46 | 47 | struct diag_client { 48 | const char *name; 49 | int fd; 50 | int in_fd; 51 | int out_fd; 52 | 53 | bool hdlc_encoded; 54 | 55 | bool enabled; 56 | 57 | struct circ_buf recv_buf; 58 | struct hdlc_decoder recv_decoder; 59 | 60 | struct list_head outq; 61 | struct list_head node; 62 | }; 63 | 64 | struct list_head diag_clients = LIST_INIT(diag_clients); 65 | 66 | /** 67 | * dm_add() - register new DM 68 | * @dm: DM object to register 69 | */ 70 | struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_encoded) 71 | { 72 | struct diag_client *dm; 73 | 74 | dm = calloc(1, sizeof(*dm)); 75 | if (!dm) 76 | err(1, "failed to allocate DM context\n"); 77 | 78 | dm->name = strdup(name); 79 | dm->in_fd = in_fd; 80 | dm->out_fd = out_fd; 81 | dm->hdlc_encoded = hdlc_encoded; 82 | list_init(&dm->outq); 83 | 84 | if (dm->in_fd >= 0) 85 | watch_add_readfd(dm->in_fd, dm_recv, dm, NULL); 86 | watch_add_writeq(dm->out_fd, &dm->outq); 87 | 88 | list_add(&diag_clients, &dm->node); 89 | 90 | /* Disable DM by default, so that */ 91 | dm->enabled = false; 92 | 93 | return dm; 94 | } 95 | 96 | static int dm_recv_hdlc(struct diag_client *dm) 97 | { 98 | size_t msglen; 99 | ssize_t n; 100 | void *msg; 101 | int ret = 0; 102 | 103 | for (;;) { 104 | n = circ_read(dm->in_fd, &dm->recv_buf); 105 | if (n < 0 && errno == EAGAIN) { 106 | break; 107 | } else if (n < 0) { 108 | ret = -errno; 109 | warn("Failed to read from %s\n", dm->name); 110 | break; 111 | } 112 | 113 | for (;;) { 114 | msg = hdlc_decode_one(&dm->recv_decoder, &dm->recv_buf, 115 | &msglen); 116 | if (!msg) 117 | break; 118 | 119 | diag_client_handle_command(dm, msg, msglen); 120 | } 121 | } 122 | 123 | return ret; 124 | } 125 | 126 | static int dm_recv_raw(struct diag_client *dm) 127 | { 128 | int saved_errno; 129 | unsigned char buf[4096]; 130 | ssize_t n; 131 | 132 | for (;;) { 133 | n = read(dm->in_fd, buf, sizeof(buf)); 134 | if (!n) { 135 | watch_remove_fd(dm->in_fd); 136 | break; 137 | } else if (n < 0 && errno == EAGAIN) { 138 | break; 139 | } else if (n < 0) { 140 | saved_errno = -errno; 141 | warn("Failed to read from %s\n", dm->name); 142 | return saved_errno; 143 | } 144 | 145 | diag_client_handle_command(dm, buf, n); 146 | } 147 | 148 | return 0; 149 | } 150 | 151 | /** 152 | * dm_recv() - read and handle data from a DM 153 | * @fd: the file descriptor associated with the DM 154 | * @data: private data, must be a diag_client object 155 | */ 156 | int dm_recv(int fd, void* data) 157 | { 158 | struct diag_client *dm = (struct diag_client *)data; 159 | 160 | if (dm->hdlc_encoded) 161 | return dm_recv_hdlc(dm); 162 | else 163 | return dm_recv_raw(dm); 164 | } 165 | 166 | static ssize_t dm_send_flow(struct diag_client *dm, const void *ptr, size_t len, 167 | struct watch_flow *flow) 168 | { 169 | if (!dm->enabled) 170 | return 0; 171 | 172 | if (dm->hdlc_encoded) 173 | hdlc_enqueue_flow(&dm->outq, ptr, len, flow); 174 | else 175 | queue_push_flow(&dm->outq, ptr, len, flow); 176 | 177 | return 0; 178 | } 179 | 180 | /** 181 | * dm_send() - enqueue message to DM 182 | * @dm: dm to be receiving the message 183 | * @ptr: pointer to raw message to be sent 184 | * @len: length of message 185 | */ 186 | ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len) 187 | { 188 | return dm_send_flow(dm, ptr, len, NULL); 189 | } 190 | 191 | /** 192 | * dm_broadcast() - send message to all registered DMs 193 | * @ptr: pointer to raw message to be sent 194 | * @len: length of message 195 | * @flow: flow control context for the peripheral 196 | */ 197 | void dm_broadcast(const void *ptr, size_t len, struct watch_flow *flow) 198 | { 199 | struct diag_client *dm; 200 | struct list_head *item; 201 | 202 | list_for_each(item, &diag_clients) { 203 | dm = container_of(item, struct diag_client, node); 204 | 205 | dm_send_flow(dm, ptr, len, flow); 206 | } 207 | } 208 | 209 | void dm_enable(struct diag_client *dm) 210 | { 211 | dm->enabled = true; 212 | } 213 | 214 | void dm_disable(struct diag_client *dm) 215 | { 216 | dm->enabled = false; 217 | 218 | /* XXX: purge dm->outq */ 219 | } 220 | -------------------------------------------------------------------------------- /router/dm.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016-2018, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #ifndef __DM_H__ 33 | #define __DM_H__ 34 | 35 | #include "diag.h" 36 | 37 | struct diag_client; 38 | 39 | struct diag_client *dm_add(const char *name, int in_fd, int out_fd, bool hdlc_encoded); 40 | int dm_recv(int fd, void* data); 41 | ssize_t dm_send(struct diag_client *dm, const void *ptr, size_t len); 42 | void dm_broadcast(const void *ptr, size_t len, struct watch_flow *flow); 43 | void dm_enable(struct diag_client *dm); 44 | void dm_disable(struct diag_client *dm); 45 | 46 | #endif 47 | 48 | -------------------------------------------------------------------------------- /router/hdlc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #include 32 | #include 33 | 34 | #include "hdlc.h" 35 | #include "util.h" 36 | 37 | /* 38 | * HDLC frame check is performed by a reversed CRC-CCITT-16 with polynomial 39 | * x^16 + x^12 + x^5 + 1 (0x8408), an initial value of 0xffff and the result 40 | * XORed with 0xffff. 41 | * 42 | * Calculation is performed with a table lookup as described in 43 | * http://www.ross.net/crc/download/crc_v3.txt 44 | */ 45 | static uint16_t crc_table[256] = { 46 | 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 47 | 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 48 | 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, 49 | 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, 50 | 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 51 | 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 52 | 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 53 | 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, 54 | 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 55 | 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 56 | 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014, 57 | 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 58 | 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 59 | 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 60 | 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 61 | 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, 62 | 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 63 | 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 64 | 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50, 65 | 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, 66 | 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 67 | 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 68 | 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 69 | 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, 70 | 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 71 | 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 72 | 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, 73 | 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 74 | 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 75 | }; 76 | 77 | static uint16_t hdlc_crc_byte(uint16_t crc, uint8_t ch) 78 | { 79 | return (crc >> 8) ^ crc_table[(crc ^ ch) & 0xff]; 80 | } 81 | 82 | void *hdlc_encode(const void *src, size_t slen, size_t *dlen) 83 | { 84 | const uint8_t *end = src + slen; 85 | const uint8_t *s = src; 86 | uint16_t crc = 0xffff; 87 | uint8_t tmp[2]; 88 | uint8_t *dst; 89 | uint8_t *d; 90 | int i; 91 | 92 | dst = malloc((slen + 2) * 2 + 1); 93 | if (!dst) 94 | return NULL; 95 | 96 | d = dst; 97 | while (s < end) { 98 | crc = hdlc_crc_byte(crc, *s); 99 | 100 | if (*s == 0x7d || *s == 0x7e) { 101 | *d++ = 0x7d; 102 | *d++ = *s++ ^ 0x20; 103 | } else { 104 | *d++ = *s++; 105 | } 106 | } 107 | 108 | tmp[0] = ~crc & 0xff; 109 | tmp[1] = ~crc >> 8; 110 | 111 | s = tmp; 112 | for (i = 0; i < 2; i++) { 113 | if (*s == 0x7d || *s == 0x7e) { 114 | *d++ = 0x7d; 115 | *d++ = *s++ ^ 0x20; 116 | } else { 117 | *d++ = *s++; 118 | } 119 | } 120 | 121 | *d++ = 0x7e; 122 | *dlen = d - dst; 123 | 124 | return dst; 125 | } 126 | 127 | void *hdlc_decode_one(struct hdlc_decoder *hdlc, struct circ_buf *buf, 128 | size_t *msglen) 129 | { 130 | uint8_t ch; 131 | 132 | if (!hdlc->raw) 133 | hdlc->raw = hdlc->raw_buf; 134 | 135 | for (;;) { 136 | if (buf->tail == buf->head) 137 | return 0; 138 | 139 | ch = buf->buf[buf->tail]; 140 | buf->tail = (buf->tail + 1) & (HDLC_BUF_SIZE - 1); 141 | 142 | if (ch == 0x7e) { 143 | break; 144 | } else if (ch == 0x7d) { 145 | hdlc->escape = 0x20; 146 | } else { 147 | *hdlc->raw++ = ch ^ hdlc->escape; 148 | hdlc->escape = 0; 149 | } 150 | 151 | } 152 | 153 | if (hdlc->raw == hdlc->raw_buf) 154 | return NULL; 155 | 156 | hdlc->raw--; 157 | hdlc->raw--; 158 | 159 | *msglen = hdlc->raw - hdlc->raw_buf; 160 | 161 | hdlc->raw = hdlc->raw_buf; 162 | hdlc->escape = 0; 163 | 164 | return hdlc->raw_buf; 165 | } 166 | -------------------------------------------------------------------------------- /router/hdlc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #ifndef __HDLC_H__ 33 | #define __HDLC_H__ 34 | 35 | #include 36 | 37 | #include "circ_buf.h" 38 | 39 | struct circ_buf; 40 | 41 | struct hdlc_decoder { 42 | char raw_buf[HDLC_BUF_SIZE]; 43 | char *raw; 44 | 45 | uint8_t escape; 46 | }; 47 | 48 | void *hdlc_encode(const void *src, size_t slen, size_t *dlen); 49 | 50 | void *hdlc_decode_one(struct hdlc_decoder *hdlc, struct circ_buf *buf, 51 | size_t *msglen); 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /router/list.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __LIST_H__ 32 | #define __LIST_H__ 33 | 34 | #include 35 | #include 36 | 37 | #ifndef container_of 38 | #define container_of(ptr, type, member) ({ \ 39 | const typeof(((type *)0)->member)*__mptr = (ptr); \ 40 | (type *)((char *)__mptr - offsetof(type, member)); \ 41 | }) 42 | #endif 43 | 44 | struct list_head { 45 | struct list_head *prev; 46 | struct list_head *next; 47 | }; 48 | 49 | #define LIST_INIT(list) { &(list), &(list) } 50 | 51 | static inline void list_init(struct list_head *list) 52 | { 53 | list->prev = list->next = list; 54 | } 55 | 56 | static inline bool list_empty(struct list_head *list) 57 | { 58 | return list->next == list; 59 | } 60 | 61 | static inline void list_add(struct list_head *list, struct list_head *item) 62 | { 63 | struct list_head *prev = list->prev; 64 | 65 | item->next = list; 66 | item->prev = prev; 67 | 68 | prev->next = list->prev = item; 69 | } 70 | 71 | static inline void list_del(struct list_head *item) 72 | { 73 | item->prev->next = item->next; 74 | item->next->prev = item->prev; 75 | } 76 | 77 | #define list_for_each(item, list) \ 78 | for (item = (list)->next; item != list; item = item->next) 79 | 80 | #define list_for_each_safe(item, next, list) \ 81 | for (item = (list)->next, next = item->next; item != list; item = next, next = item->next) 82 | 83 | #define list_entry(item, type, member) \ 84 | container_of(item, type, member) 85 | 86 | #define list_entry_first(list, type, member) \ 87 | container_of((list)->next, type, member) 88 | 89 | #define list_entry_next(item, member) \ 90 | container_of((item)->member.next, typeof(*(item)), member) 91 | 92 | #define list_for_each_entry(item, list, member) \ 93 | for (item = list_entry_first(list, typeof(*(item)), member); \ 94 | &item->member != list; \ 95 | item = list_entry_next(item, member)) 96 | 97 | #define list_for_each_entry_safe(item, next, list, member) \ 98 | for (item = list_entry_first(list, typeof(*(item)), member), \ 99 | next = list_entry_next(item, member); \ 100 | &item->member != list; \ 101 | item = next, \ 102 | next = list_entry_next(item, member)) \ 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /router/masks.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution. 13 | * * Neither the name of The Linux Foundation nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 18 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 21 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 27 | * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "diag.h" 36 | #include "masks.h" 37 | #include "peripheral.h" 38 | #include "util.h" 39 | 40 | #define APPS_BUF_SIZE 16384 41 | 42 | struct diag_mask_info { 43 | void *ptr; 44 | int mask_len; 45 | uint8_t status; 46 | }__packed; 47 | 48 | static struct diag_mask_info msg_mask; 49 | static struct diag_mask_info msg_bt_mask; 50 | static struct diag_mask_info log_mask; 51 | static struct diag_mask_info event_mask; 52 | 53 | uint16_t event_max_num_bits; 54 | 55 | static int diag_mask_init(struct diag_mask_info *mask_info, int mask_len, 56 | int update_buf_len) 57 | { 58 | if (!mask_info || mask_len < 0 || update_buf_len < 0) 59 | return -EINVAL; 60 | 61 | mask_info->status = DIAG_CTRL_MASK_INVALID; 62 | mask_info->mask_len = mask_len; 63 | if (mask_len > 0) { 64 | mask_info->ptr = malloc(mask_len); 65 | if (!mask_info->ptr) 66 | return -ENOMEM; 67 | memset(mask_info->ptr, 0, mask_len); 68 | } 69 | 70 | return 0; 71 | } 72 | 73 | static int diag_create_msg_mask_table_entry(struct diag_msg_mask_t *msg_mask, 74 | uint32_t entry) 75 | { 76 | if (!msg_mask) 77 | return -EIO; 78 | 79 | if (entry > NUM_OF_MASK_RANGES) 80 | return -EINVAL; 81 | 82 | msg_mask->ssid_first = ssid_first_arr[entry]; 83 | msg_mask->ssid_last = ssid_last_arr[entry]; 84 | msg_mask->ssid_last_tools = ssid_last_arr[entry]; 85 | msg_mask->range = msg_mask->ssid_last - msg_mask->ssid_first + 1; 86 | 87 | if (msg_mask->range < MAX_SSID_PER_RANGE) 88 | msg_mask->range = MAX_SSID_PER_RANGE; 89 | msg_mask->range_tools = msg_mask->range; 90 | 91 | if (msg_mask->range > 0) { 92 | msg_mask->ptr = malloc(msg_mask->range * sizeof(uint32_t)); 93 | if (!msg_mask->ptr) 94 | return -ENOMEM; 95 | memset(msg_mask->ptr, 0xFF, msg_mask->range * sizeof(uint32_t)); 96 | } 97 | 98 | return 0; 99 | } 100 | 101 | static int diag_create_build_time_mask() 102 | { 103 | int err = 0; 104 | struct diag_msg_mask_t *build_mask = msg_bt_mask.ptr; 105 | int i; 106 | 107 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, build_mask++) { 108 | err = diag_create_msg_mask_table_entry(build_mask, i); 109 | if (err) 110 | break; 111 | } 112 | 113 | return err; 114 | } 115 | 116 | static int diag_create_msg_mask_table() 117 | { 118 | int err = 0; 119 | struct diag_msg_mask_t *mask = msg_mask.ptr; 120 | int i; 121 | 122 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, mask++) { 123 | err = diag_create_msg_mask_table_entry(mask, i); 124 | if (err) 125 | break; 126 | } 127 | 128 | return err; 129 | } 130 | 131 | static int diag_msg_mask_init() 132 | { 133 | int ret; 134 | 135 | ret = diag_mask_init(&msg_mask, MSG_MASK_SIZE, APPS_BUF_SIZE); 136 | if (ret) 137 | return ret; 138 | 139 | ret = diag_create_msg_mask_table(); 140 | if (ret) { 141 | printf("diag: Unable to create msg masks, err: %d\n", ret); 142 | return ret; 143 | } 144 | 145 | return ret; 146 | } 147 | 148 | static void diag_msg_mask_exit() 149 | { 150 | struct diag_msg_mask_t *mask = msg_mask.ptr; 151 | int i; 152 | 153 | if (mask) { 154 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, mask++) 155 | free(mask->ptr); 156 | free(msg_mask.ptr); 157 | } 158 | } 159 | 160 | static int diag_build_time_mask_init() 161 | { 162 | int err; 163 | 164 | /* There is no need for update buffer for Build Time masks */ 165 | err = diag_mask_init(&msg_bt_mask, MSG_MASK_SIZE, 0); 166 | if (err) 167 | return err; 168 | 169 | err = diag_create_build_time_mask(); 170 | if (err) { 171 | printf("diag: Unable to create msg build time masks, err: %d\n", err); 172 | return err; 173 | } 174 | 175 | return 0; 176 | } 177 | 178 | static void diag_build_time_mask_exit(void) 179 | { 180 | struct diag_msg_mask_t *mask = msg_bt_mask.ptr; 181 | int i; 182 | 183 | if (mask) { 184 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, mask++) 185 | free(mask->ptr); 186 | free(msg_bt_mask.ptr); 187 | } 188 | } 189 | 190 | static int diag_create_log_mask_table(void) 191 | { 192 | struct diag_log_mask_t *mask = log_mask.ptr; 193 | uint8_t equip_id; 194 | int err = 0; 195 | 196 | for (equip_id = 0; equip_id < MAX_EQUIP_ID; equip_id++, mask++) { 197 | mask->equip_id = equip_id; 198 | mask->num_items = LOG_GET_ITEM_NUM(log_code_last_tbl[equip_id]); 199 | mask->num_items_tools = mask->num_items; 200 | if (BITS_TO_BYTES(mask->num_items) > MAX_ITEMS_PER_EQUIP_ID) 201 | mask->range = BITS_TO_BYTES(mask->num_items); 202 | else 203 | mask->range = MAX_ITEMS_PER_EQUIP_ID; 204 | mask->range_tools = mask->range; 205 | mask->ptr = malloc(mask->range); 206 | if (!mask->ptr) { 207 | err = -ENOMEM; 208 | break; 209 | } 210 | memset(mask->ptr, 0, mask->range); 211 | } 212 | 213 | return err; 214 | } 215 | 216 | static int diag_log_mask_init(void) 217 | { 218 | int ret; 219 | 220 | ret = diag_mask_init(&log_mask, LOG_MASK_SIZE, APPS_BUF_SIZE); 221 | if (ret) 222 | return ret; 223 | 224 | return diag_create_log_mask_table(); 225 | } 226 | 227 | static void diag_log_mask_exit(void) 228 | { 229 | struct diag_log_mask_t *mask = log_mask.ptr; 230 | int i; 231 | 232 | if (mask) { 233 | for (i = 0; i < MAX_EQUIP_ID; i++, mask++) 234 | free(mask->ptr); 235 | free(log_mask.ptr); 236 | } 237 | } 238 | 239 | static int diag_event_mask_init(void) 240 | { 241 | event_max_num_bits = APPS_EVENT_LAST_ID; 242 | 243 | return diag_mask_init(&event_mask, EVENT_MASK_SIZE, APPS_BUF_SIZE); 244 | } 245 | 246 | static void diag_event_mask_exit(void) 247 | { 248 | free(event_mask.ptr); 249 | } 250 | 251 | int diag_masks_init() 252 | { 253 | if (diag_msg_mask_init() || 254 | diag_build_time_mask_init() || 255 | diag_log_mask_init() || 256 | diag_event_mask_init()) { 257 | diag_masks_exit(); 258 | printf("diag: Could not initialize diag mask buffers\n"); 259 | 260 | return -ENOMEM; 261 | } 262 | 263 | return 0; 264 | } 265 | 266 | void diag_masks_exit() 267 | { 268 | diag_msg_mask_exit(); 269 | diag_build_time_mask_exit(); 270 | diag_log_mask_exit(); 271 | diag_event_mask_exit(); 272 | } 273 | 274 | uint8_t diag_get_log_mask_status() 275 | { 276 | return log_mask.status; 277 | } 278 | 279 | void diag_cmd_disable_log() 280 | { 281 | struct diag_log_mask_t *log_item = log_mask.ptr; 282 | int i; 283 | 284 | for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) { 285 | memset(log_item->ptr, 0, log_item->range); 286 | } 287 | log_mask.status = DIAG_CTRL_MASK_ALL_DISABLED; 288 | } 289 | 290 | void diag_cmd_get_log_range(uint32_t *ranges, uint32_t count) 291 | { 292 | struct diag_log_mask_t *log_item = log_mask.ptr; 293 | int i; 294 | 295 | for (i = 0; i < MIN(MAX_EQUIP_ID, count); i++, log_item++) { 296 | ranges[i] = log_item->num_items_tools; 297 | } 298 | } 299 | 300 | int diag_cmd_set_log_mask(uint8_t equip_id, uint32_t *num_items, uint8_t *mask, uint32_t *mask_size) 301 | { 302 | struct diag_log_mask_t *log_item = log_mask.ptr; 303 | void *tmp_buf; 304 | int i; 305 | 306 | for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) { 307 | if (log_item->equip_id != equip_id) 308 | continue; 309 | 310 | #if 0 311 | diag_dbg(DIAG_DBG_MASKS, "Found equip_id=%d\n" 312 | "current num_items=%u range=%u\n" 313 | "request num_items=%u range=%u\n", 314 | log_item->equip_id, 315 | log_item->num_items_tools, log_item->range_tools, 316 | *num_items, BITS_TO_BYTES(*num_items)); 317 | #endif 318 | 319 | log_item->num_items_tools = MIN(*num_items, MAX_ITEMS_ALLOWED); 320 | *mask_size = BITS_TO_BYTES(log_item->num_items_tools); 321 | memset(log_item->ptr, 0, log_item->range_tools); 322 | 323 | if (*mask_size > log_item->range_tools) { 324 | tmp_buf = realloc(log_item->ptr, *mask_size); 325 | if (!tmp_buf) { 326 | log_mask.status = DIAG_CTRL_MASK_INVALID; 327 | warn("Failed to reallocate log mask\n"); 328 | 329 | return -errno; 330 | } 331 | log_item->ptr = tmp_buf; 332 | memset(log_item->ptr, 0, *mask_size); 333 | log_item->range_tools = *mask_size; 334 | } 335 | *num_items = log_item->num_items_tools; 336 | memcpy(log_item->ptr, mask, *mask_size); 337 | log_mask.status = DIAG_CTRL_MASK_VALID; 338 | 339 | return 0; 340 | } 341 | 342 | return 1; 343 | } 344 | 345 | int diag_cmd_get_log_mask(uint32_t equip_id, uint32_t *num_items, uint8_t ** mask, uint32_t *mask_size) 346 | { 347 | struct diag_log_mask_t *log_item = log_mask.ptr; 348 | int i; 349 | 350 | for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) { 351 | if (log_item->equip_id != equip_id) { 352 | continue; 353 | } 354 | 355 | *num_items = log_item->num_items_tools; 356 | *mask_size = BITS_TO_BYTES(log_item->num_items_tools); 357 | *mask = malloc(*mask_size); 358 | if (!*mask) { 359 | warn("Failed to allocate log mask\n"); 360 | 361 | return -errno; 362 | } 363 | memcpy(*mask, log_item->ptr, *mask_size); 364 | 365 | return 0; 366 | } 367 | 368 | return 1; 369 | } 370 | 371 | void diag_cmd_get_ssid_range(uint32_t *count, struct diag_ssid_range_t **ranges) 372 | { 373 | struct diag_msg_mask_t *msg_item = msg_mask.ptr; 374 | struct diag_ssid_range_t *range; 375 | int i; 376 | 377 | *count = MSG_MASK_TBL_CNT; 378 | *ranges = calloc(*count, sizeof(**ranges)); 379 | if (!*ranges) { 380 | warn("Failed to allocate ssid ranges\n"); 381 | 382 | return; 383 | } 384 | 385 | range = *ranges; 386 | for (i = 0; i < *count; i++, msg_item++, range++) { 387 | range->ssid_first = msg_item->ssid_first; 388 | range->ssid_last = msg_item->ssid_last_tools; 389 | } 390 | } 391 | 392 | uint8_t diag_get_build_mask_status() 393 | { 394 | return msg_bt_mask.status; 395 | } 396 | 397 | int diag_cmd_get_build_mask(struct diag_ssid_range_t *range, uint32_t **mask) 398 | { 399 | struct diag_msg_mask_t *msg_item = msg_bt_mask.ptr; 400 | uint32_t num_entries = 0; 401 | uint32_t mask_size = 0; 402 | int i; 403 | 404 | for (i = 0;i < MSG_MASK_TBL_CNT; i++, msg_item++) { 405 | if (msg_item->ssid_first != range->ssid_first) { 406 | continue; 407 | } 408 | num_entries = range->ssid_last - range->ssid_first + 1; 409 | if (num_entries > msg_item->range) { 410 | warn("diag: Truncating ssid range for ssid_first: %d ssid_last %d\n", 411 | range->ssid_first, range->ssid_last); 412 | num_entries = msg_item->range; 413 | range->ssid_last = range->ssid_first + msg_item->range; 414 | } 415 | mask_size = num_entries * sizeof(uint32_t); 416 | *mask = malloc(mask_size); 417 | if (!*mask) { 418 | warn("Failed to allocate build mask\n"); 419 | 420 | return -errno; 421 | } 422 | memcpy(*mask, msg_item->ptr, mask_size); 423 | 424 | return 0; 425 | } 426 | 427 | return 1; 428 | } 429 | 430 | uint8_t diag_get_msg_mask_status() 431 | { 432 | return msg_mask.status; 433 | } 434 | 435 | int diag_cmd_get_msg_mask(struct diag_ssid_range_t *range, uint32_t **mask) 436 | { 437 | struct diag_msg_mask_t *msg_item = msg_mask.ptr; 438 | uint32_t mask_size = 0; 439 | int i; 440 | 441 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, msg_item++) { 442 | if ((range->ssid_first < msg_item->ssid_first) || 443 | (range->ssid_first > msg_item->ssid_last_tools)) { 444 | continue; 445 | } 446 | 447 | mask_size = msg_item->range * sizeof(**mask); 448 | range->ssid_first = msg_item->ssid_first; 449 | range->ssid_last = msg_item->ssid_last; 450 | *mask = malloc(mask_size); 451 | if (!*mask) { 452 | warn("Failed to allocate event mask\n"); 453 | 454 | return -errno; 455 | } 456 | memcpy(*mask, msg_item->ptr, mask_size); 457 | 458 | return 0; 459 | } 460 | 461 | return 1; 462 | } 463 | 464 | int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, const uint32_t *mask) 465 | { 466 | struct diag_msg_mask_t *msg_item = msg_mask.ptr; 467 | uint32_t num_msgs = 0; 468 | struct diag_msg_mask_t *mask_next = NULL; 469 | uint32_t offset = 0; 470 | void *tmp_buf; 471 | int i; 472 | 473 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, msg_item++) { 474 | if (i < (MSG_MASK_TBL_CNT - 1)) { 475 | mask_next = msg_item; 476 | mask_next++; 477 | } else{ 478 | mask_next = NULL; 479 | } 480 | 481 | if ((range.ssid_first < msg_item->ssid_first) || 482 | (range.ssid_first > msg_item->ssid_first + MAX_SSID_PER_RANGE) || 483 | (mask_next && (range.ssid_first >= mask_next->ssid_first))) { 484 | continue; 485 | } 486 | 487 | mask_next = NULL; 488 | num_msgs = range.ssid_last - range.ssid_first + 1; 489 | if (num_msgs > MAX_SSID_PER_RANGE) { 490 | warn("diag: Truncating ssid range, %d-%d to max allowed: %d\n", 491 | msg_item->ssid_first, msg_item->ssid_last, 492 | MAX_SSID_PER_RANGE); 493 | num_msgs = MAX_SSID_PER_RANGE; 494 | msg_item->range_tools = MAX_SSID_PER_RANGE; 495 | msg_item->ssid_last_tools = msg_item->ssid_first + msg_item->range_tools; 496 | } 497 | if (range.ssid_last > msg_item->ssid_last_tools) { 498 | if (num_msgs != MAX_SSID_PER_RANGE) 499 | msg_item->ssid_last_tools = range.ssid_last; 500 | msg_item->range_tools = msg_item->ssid_last_tools - msg_item->ssid_first + 1; 501 | tmp_buf = realloc(msg_item->ptr, msg_item->range_tools * sizeof(*mask)); 502 | if (!tmp_buf) { 503 | msg_mask.status = DIAG_CTRL_MASK_INVALID; 504 | warn("Failed to reallocate msg mask\n"); 505 | 506 | return -errno; 507 | } 508 | msg_item->ptr = tmp_buf; 509 | } 510 | 511 | offset = range.ssid_first - msg_item->ssid_first; 512 | if (offset + num_msgs > msg_item->range_tools) { 513 | warn("diag: Not in msg mask range, num_msgs: %d, offset: %d\n", 514 | num_msgs, offset); 515 | 516 | return 1; 517 | } 518 | memcpy(msg_item->ptr + offset, mask, num_msgs * sizeof(*mask)); 519 | msg_mask.status = DIAG_CTRL_MASK_VALID; 520 | 521 | return 0; 522 | } 523 | 524 | return 1; 525 | } 526 | 527 | void diag_cmd_set_all_msg_mask(uint32_t mask) 528 | { 529 | struct diag_msg_mask_t *msg_item = msg_mask.ptr; 530 | int i; 531 | 532 | msg_mask.status = mask ? DIAG_CTRL_MASK_ALL_ENABLED : 533 | DIAG_CTRL_MASK_ALL_DISABLED; 534 | for (i = 0; i < MSG_MASK_TBL_CNT; i++, msg_item++) { 535 | memset(msg_item->ptr , mask , msg_item->range_tools * sizeof(mask)); 536 | } 537 | } 538 | 539 | uint8_t diag_get_event_mask_status() 540 | { 541 | return event_mask.status; 542 | } 543 | 544 | int diag_cmd_get_event_mask(uint16_t num_bits, uint8_t **mask) 545 | { 546 | uint32_t mask_size = BITS_TO_BYTES(num_bits); 547 | 548 | if (num_bits > event_max_num_bits) { 549 | return 1; 550 | } 551 | 552 | *mask = malloc(mask_size); 553 | if (!*mask) { 554 | warn("Failed to allocate event mask\n"); 555 | 556 | return -errno; 557 | } 558 | memcpy(*mask, event_mask.ptr, mask_size); 559 | 560 | return 0; 561 | } 562 | 563 | int diag_cmd_update_event_mask(uint16_t num_bits, const uint8_t *mask) 564 | { 565 | void *tmp_buf; 566 | 567 | if (num_bits > event_max_num_bits ) { 568 | tmp_buf = realloc(event_mask.ptr, BITS_TO_BYTES(num_bits)); 569 | if (!tmp_buf) { 570 | event_mask.status = DIAG_CTRL_MASK_INVALID; 571 | warn("Failed to reallocate event mask\n"); 572 | 573 | return -errno; 574 | } 575 | 576 | event_mask.ptr = tmp_buf; 577 | event_max_num_bits = num_bits; 578 | event_mask.mask_len = BITS_TO_BYTES(num_bits); 579 | } 580 | memcpy(event_mask.ptr, mask, BITS_TO_BYTES(num_bits)); 581 | event_mask.status = DIAG_CTRL_MASK_VALID; 582 | 583 | return 0; 584 | } 585 | 586 | void diag_cmd_toggle_events(bool enabled) 587 | { 588 | if (enabled) { 589 | memset(event_mask.ptr, 0xff, event_mask.mask_len); 590 | event_mask.status = DIAG_CTRL_MASK_ALL_ENABLED; 591 | } else { 592 | memset(event_mask.ptr, 0x00, event_mask.mask_len); 593 | event_mask.status = DIAG_CTRL_MASK_ALL_DISABLED; 594 | } 595 | } 596 | -------------------------------------------------------------------------------- /router/masks.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution. 13 | * * Neither the name of The Linux Foundation nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 18 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 21 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 27 | * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef MASKS_H_ 30 | #define MASKS_H_ 31 | 32 | #include "diag.h" 33 | #include "util.h" 34 | 35 | #ifndef __packed 36 | #define __packed __attribute__((__packed__)) 37 | #endif 38 | 39 | #define MSG_MASKS_TYPE 0x00000001 40 | #define LOG_MASKS_TYPE 0x00000002 41 | #define EVENT_MASKS_TYPE 0x00000004 42 | 43 | #define MSG_MASK_TBL_CNT 25 44 | #define APPS_EVENT_LAST_ID 0x0B3F 45 | 46 | #define MAX_SSID_PER_RANGE 200 47 | 48 | extern uint16_t event_max_num_bits; 49 | 50 | /* LOG CODES */ 51 | static const uint32_t log_code_last_tbl[] = { 52 | 0x0, /* EQUIP ID 0 */ 53 | 0x1A02, /* EQUIP ID 1 */ 54 | 0x0, /* EQUIP ID 2 */ 55 | 0x0, /* EQUIP ID 3 */ 56 | 0x4910, /* EQUIP ID 4 */ 57 | 0x5420, /* EQUIP ID 5 */ 58 | 0x0, /* EQUIP ID 6 */ 59 | 0x74FF, /* EQUIP ID 7 */ 60 | 0x0, /* EQUIP ID 8 */ 61 | 0x0, /* EQUIP ID 9 */ 62 | 0xA38A, /* EQUIP ID 10 */ 63 | 0xB201, /* EQUIP ID 11 */ 64 | 0x0, /* EQUIP ID 12 */ 65 | 0xD1FF, /* EQUIP ID 13 */ 66 | 0x0, /* EQUIP ID 14 */ 67 | 0x0, /* EQUIP ID 15 */ 68 | }; 69 | 70 | #define NUM_OF_MASK_RANGES 25 71 | static const uint32_t ssid_first_arr[NUM_OF_MASK_RANGES] = 72 | { 0, 500, 1000, 2000, 3000, 4000, 4500, 4600, 73 | 5000, 5500, 6000, 6500, 7000, 7100, 7200, 8000, 74 | 8500, 9000, 9500, 10200, 10251, 10300, 10350, 10400, 75 | 0xC000 }; 76 | 77 | static const uint32_t ssid_last_arr[NUM_OF_MASK_RANGES] = 78 | { 120, 506, 1007, 2008, 3014, 4010, 4573, 4615, 79 | 5033, 5516, 6081, 6521, 7003, 7111, 7201, 8000, 80 | 8529, 9008, 9510, 10210, 10255, 10300, 10377, 10416, 81 | 0xC063 }; 82 | 83 | #define LOG_GET_ITEM_NUM(xx_code) (xx_code & 0x0FFF) 84 | #define LOG_GET_EQUIP_ID(xx_code) ((xx_code & 0xF000) >> 12) 85 | #define MSG_RANGE_TO_SIZE(range) (((range).ssid_last - (range).ssid_first +1) * 4) 86 | 87 | #define MAX_EQUIP_ID 16 88 | #define EVENT_MASK_SIZE 513 89 | #define MAX_ITEMS_PER_EQUIP_ID 512 90 | #define MAX_ITEMS_ALLOWED 0xFFF 91 | 92 | #define DIAG_CTRL_MASK_INVALID 0 93 | #define DIAG_CTRL_MASK_ALL_DISABLED 1 94 | #define DIAG_CTRL_MASK_ALL_ENABLED 2 95 | #define DIAG_CTRL_MASK_VALID 3 96 | 97 | struct diag_log_mask_t { 98 | uint8_t equip_id; 99 | uint32_t num_items; 100 | uint32_t num_items_tools; 101 | uint32_t range; 102 | uint32_t range_tools; 103 | uint8_t *ptr; 104 | }__packed; 105 | 106 | struct diag_ssid_range_t { 107 | uint16_t ssid_first; 108 | uint16_t ssid_last; 109 | }__packed; 110 | 111 | struct diag_msg_mask_t { 112 | uint32_t ssid_first; 113 | uint32_t ssid_last; 114 | uint32_t ssid_last_tools; 115 | uint32_t range; 116 | uint32_t range_tools; 117 | uint32_t *ptr; 118 | }__packed; 119 | 120 | #define MSG_MASK_SIZE (MSG_MASK_TBL_CNT * sizeof(struct diag_msg_mask_t)) 121 | #define LOG_MASK_SIZE (MAX_EQUIP_ID * sizeof(struct diag_log_mask_t)) 122 | 123 | int diag_masks_init(void); 124 | void diag_masks_exit(void); 125 | 126 | uint8_t diag_get_log_mask_status(); 127 | void diag_cmd_disable_log(); 128 | void diag_cmd_get_log_range(uint32_t *ranges, uint32_t count); 129 | int diag_cmd_set_log_mask(uint8_t equip_id, uint32_t *num_items, uint8_t *mask, uint32_t *mask_size); 130 | int diag_cmd_get_log_mask(uint32_t equip_id, uint32_t *num_items, uint8_t ** mask, uint32_t *mask_size); 131 | 132 | uint8_t diag_get_build_mask_status(); 133 | void diag_cmd_get_ssid_range(uint32_t *count, struct diag_ssid_range_t **ranges); 134 | int diag_cmd_get_build_mask(struct diag_ssid_range_t *range, uint32_t **mask); 135 | 136 | uint8_t diag_get_msg_mask_status(); 137 | int diag_cmd_get_msg_mask(struct diag_ssid_range_t *range, uint32_t **mask); 138 | int diag_cmd_set_msg_mask(struct diag_ssid_range_t range, const uint32_t *mask); 139 | void diag_cmd_set_all_msg_mask(uint32_t mask); 140 | 141 | uint8_t diag_get_event_mask_status(); 142 | int diag_cmd_get_event_mask(uint16_t num_bits, uint8_t **mask); 143 | int diag_cmd_update_event_mask(uint16_t num_bits, const uint8_t *mask); 144 | void diag_cmd_toggle_events(bool enabled); 145 | 146 | #endif /* MASKS_H_ */ 147 | -------------------------------------------------------------------------------- /router/mbuf.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #include 32 | #include 33 | #include "mbuf.h" 34 | 35 | struct mbuf *mbuf_alloc(size_t size) 36 | { 37 | struct mbuf *mbuf; 38 | 39 | mbuf = malloc(sizeof(*mbuf) + size); 40 | if (!mbuf) 41 | return NULL; 42 | 43 | memset(mbuf, 0, sizeof(*mbuf)); 44 | mbuf->size = size; 45 | 46 | return mbuf; 47 | } 48 | 49 | void *mbuf_put(struct mbuf *mbuf, size_t size) 50 | { 51 | void *ptr; 52 | 53 | if (mbuf->offset + size > mbuf->size) 54 | return NULL; 55 | 56 | ptr = mbuf->data + mbuf->offset; 57 | mbuf->offset += size; 58 | 59 | return ptr; 60 | } 61 | -------------------------------------------------------------------------------- /router/mbuf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __MBUF_H__ 32 | #define __MBUF_H__ 33 | 34 | #include "list.h" 35 | 36 | struct watch_flow; 37 | 38 | struct mbuf { 39 | struct list_head node; 40 | 41 | size_t size; 42 | size_t offset; 43 | 44 | struct watch_flow *flow; 45 | 46 | char data[]; 47 | }; 48 | 49 | struct mbuf *mbuf_alloc(size_t size); 50 | void *mbuf_put(struct mbuf *mbuf, size_t size); 51 | 52 | #endif 53 | -------------------------------------------------------------------------------- /router/peripheral-qrtr.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016-2018, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "diag.h" 42 | #include "diag_cntl.h" 43 | #include "dm.h" 44 | #include "peripheral-qrtr.h" 45 | #include "watch.h" 46 | #include "util.h" 47 | 48 | #define DIAG_SERVICE_ID 4097 49 | 50 | #define DIAG_INSTANCE_BASE_MODEM 0 51 | #define DIAG_INSTANCE_BASE_LPASS 64 52 | #define DIAG_INSTANCE_BASE_WCNSS 128 53 | #define DIAG_INSTANCE_BASE_SENSORS 192 54 | #define DIAG_INSTANCE_BASE_CDSP 256 55 | #define DIAG_INSTANCE_BASE_WDSP 320 56 | 57 | enum { 58 | DIAG_INSTANCE_CNTL, 59 | DIAG_INSTANCE_CMD, 60 | DIAG_INSTANCE_DATA, 61 | DIAG_INSTANCE_DCI_CMD, 62 | DIAG_INSTANCE_DCI, 63 | }; 64 | 65 | static int qrtr_cntl_recv(int fd, void *data) 66 | { 67 | struct peripheral *perif = data; 68 | struct sockaddr_qrtr sq; 69 | struct qrtr_packet pkt; 70 | socklen_t sl; 71 | uint8_t buf[4096]; 72 | ssize_t n; 73 | int ret; 74 | 75 | sl = sizeof(sq); 76 | n = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl); 77 | if (n < 0) { 78 | ret = -errno; 79 | if (ret != -ENETRESET) 80 | fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret); 81 | return ret; 82 | } 83 | 84 | ret = qrtr_decode(&pkt, buf, n, &sq); 85 | if (ret < 0) { 86 | fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n"); 87 | return ret; 88 | } 89 | 90 | switch (pkt.type) { 91 | case QRTR_TYPE_DEL_CLIENT: 92 | break; 93 | case QRTR_TYPE_DATA: 94 | if (!perif->cntl_open) { 95 | connect(perif->cntl_fd, (struct sockaddr *)&sq, sizeof(sq)); 96 | perif->cntl_open = true; 97 | watch_add_writeq(perif->cntl_fd, &perif->cntlq); 98 | } 99 | 100 | return diag_cntl_recv(perif, pkt.data, pkt.data_len); 101 | case QRTR_TYPE_BYE: 102 | watch_remove_writeq(perif->cntl_fd); 103 | perif->cntl_open = false; 104 | break; 105 | default: 106 | fprintf(stderr, "Unhandled DIAG CNTL message from %d:%d (%d)\n", 107 | pkt.node, pkt.port, pkt.type); 108 | break; 109 | } 110 | 111 | return 0; 112 | } 113 | 114 | struct non_hdlc_pkt { 115 | uint8_t start; 116 | uint8_t version; 117 | uint16_t length; 118 | char payload[]; 119 | }; 120 | 121 | static int qrtr_cmd_recv(int fd, void *data) 122 | { 123 | struct peripheral *perif = data; 124 | struct non_hdlc_pkt *frame; 125 | struct sockaddr_qrtr cmdsq; 126 | struct sockaddr_qrtr sq; 127 | struct qrtr_packet pkt; 128 | struct circ_buf *buf = &perif->recv_buf; 129 | socklen_t sl; 130 | ssize_t n; 131 | int ret; 132 | 133 | sl = sizeof(sq); 134 | n = recvfrom(fd, buf->buf, sizeof(buf->buf), 0, (void *)&sq, &sl); 135 | if (n < 0) { 136 | ret = -errno; 137 | if (ret != -ENETRESET) 138 | fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret); 139 | return ret; 140 | } 141 | 142 | ret = qrtr_decode(&pkt, buf->buf, n, &sq); 143 | if (ret < 0) { 144 | fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n"); 145 | return ret; 146 | } 147 | 148 | switch (pkt.type) { 149 | case QRTR_TYPE_DEL_CLIENT: 150 | break; 151 | case QRTR_TYPE_DATA: 152 | frame = pkt.data; 153 | if (frame->start != 0x7e || frame->version != 1) { 154 | fprintf(stderr, "invalid non-HDLC frame\n"); 155 | break; 156 | } 157 | 158 | if (sizeof(*frame) + frame->length + 1 > pkt.data_len) { 159 | fprintf(stderr, "truncated non-HDLC frame\n"); 160 | break; 161 | } 162 | 163 | if (frame->payload[frame->length] != 0x7e) { 164 | fprintf(stderr, "non-HDLC frame is not truncated\n"); 165 | break; 166 | } 167 | 168 | dm_broadcast(frame->payload, frame->length, NULL); 169 | break; 170 | case QRTR_TYPE_NEW_SERVER: 171 | if (pkt.node == 0 && pkt.port == 0) 172 | break; 173 | 174 | printf("Connecting CMD socket to %d:%d\n", pkt.node, pkt.port); 175 | cmdsq.sq_family = AF_QIPCRTR; 176 | cmdsq.sq_node = pkt.node; 177 | cmdsq.sq_port = pkt.port; 178 | 179 | ret = connect(perif->cmd_fd, (struct sockaddr *)&cmdsq, sizeof(cmdsq)); 180 | if (ret < 0) 181 | err(1, "failed to connect to %d:%d", cmdsq.sq_node, cmdsq.sq_port); 182 | watch_add_writeq(perif->cmd_fd, &perif->cmdq); 183 | break; 184 | case QRTR_TYPE_DEL_SERVER: 185 | watch_remove_writeq(perif->cmd_fd); 186 | break; 187 | default: 188 | fprintf(stderr, "Unhandled DIAG CMD message from %d:%d (%d)\n", 189 | pkt.node, pkt.port, pkt.type); 190 | break; 191 | } 192 | 193 | return 0; 194 | } 195 | 196 | static int qrtr_data_recv(int fd, void *data) 197 | { 198 | struct peripheral *perif = data; 199 | struct sockaddr_qrtr sq; 200 | struct qrtr_packet pkt; 201 | socklen_t sl; 202 | uint8_t buf[4096]; 203 | ssize_t n; 204 | int ret; 205 | struct non_hdlc_pkt *frame; 206 | 207 | sl = sizeof(sq); 208 | n = recvfrom(fd, buf, sizeof(buf), 0, (void *)&sq, &sl); 209 | if (n < 0) { 210 | ret = -errno; 211 | if (ret != -ENETRESET) 212 | fprintf(stderr, "[DIAG-QRTR] recvfrom failed: %d\n", ret); 213 | return ret; 214 | } 215 | 216 | ret = qrtr_decode(&pkt, buf, n, &sq); 217 | if (ret < 0) { 218 | fprintf(stderr, "[PD-MAPPER] unable to decode qrtr packet\n"); 219 | return ret; 220 | } 221 | 222 | switch (pkt.type) { 223 | case QRTR_TYPE_DEL_CLIENT: 224 | break; 225 | case QRTR_TYPE_DATA: 226 | if (!perif->data_open) { 227 | connect(perif->data_fd, (struct sockaddr *)&sq, sizeof(sq)); 228 | perif->data_open = true; 229 | watch_add_writeq(perif->data_fd, &perif->dataq); 230 | } 231 | frame = pkt.data; 232 | if (frame->start != 0x7e || frame->version != 1) { 233 | fprintf(stderr, "invalid non-HDLC frame\n"); 234 | break; 235 | } 236 | 237 | if (sizeof(*frame) + frame->length + 1 > pkt.data_len) { 238 | fprintf(stderr, "truncated non-HDLC frame\n"); 239 | break; 240 | } 241 | 242 | if (frame->payload[frame->length] != 0x7e) { 243 | fprintf(stderr, "non-HDLC frame is not truncated\n"); 244 | break; 245 | } 246 | dm_broadcast(frame->payload, frame->length, perif->flow); 247 | break; 248 | case QRTR_TYPE_BYE: 249 | watch_remove_writeq(perif->data_fd); 250 | perif->data_open = false; 251 | break; 252 | default: 253 | fprintf(stderr, "Unhandled DIAG DATA message from %d:%d (%d)\n", 254 | pkt.node, pkt.port, pkt.type); 255 | break; 256 | } 257 | 258 | return 0; 259 | } 260 | 261 | int qrtr_perif_send(struct peripheral *perif, const void *ptr, size_t len) 262 | { 263 | if (perif->features & DIAG_FEATURE_APPS_HDLC_ENCODE) 264 | queue_push(&perif->cmdq, ptr, len); 265 | else 266 | hdlc_enqueue(&perif->cmdq, ptr, len); 267 | 268 | return 0; 269 | } 270 | 271 | void qrtr_perif_close(struct peripheral *perif) 272 | { 273 | } 274 | 275 | static int qrtr_perif_init_subsystem(const char *name, int instance_base) 276 | { 277 | struct peripheral *perif; 278 | struct watch_flow *flow; 279 | 280 | perif = calloc(1, sizeof(*perif)); 281 | 282 | flow = watch_flow_new(); 283 | 284 | perif->name = strdup(name); 285 | perif->send = qrtr_perif_send; 286 | perif->close = qrtr_perif_close; 287 | perif->sockets = true; 288 | perif->flow = flow; 289 | 290 | list_init(&perif->cmdq); 291 | list_init(&perif->cntlq); 292 | list_init(&perif->dataq); 293 | 294 | perif->cntl_fd = qrtr_open(0); 295 | if (perif->cntl_fd < 0) 296 | err(1, "failed to create control socket"); 297 | 298 | perif->data_fd = qrtr_open(0); 299 | if (perif->data_fd < 0) 300 | err(1, "failed to create data socket"); 301 | 302 | perif->cmd_fd = qrtr_open(0); 303 | if (perif->cmd_fd < 0) 304 | err(1, "failed to create command socket"); 305 | 306 | perif->dci_cmd_fd = qrtr_open(0); 307 | if (perif->dci_cmd_fd < 0) 308 | err(1, "failed to create dci command socket"); 309 | 310 | /* 311 | * DIAG does not use the normal packing of "instance << 8 | version" in 312 | * the one 32-bit "instance" field of the service notifications, so 313 | * pass the DIAG instance information as "version" into these functions 314 | * instead. 315 | */ 316 | qrtr_publish(perif->cntl_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_CNTL, 0); 317 | qrtr_new_lookup(perif->cmd_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_CMD, 0); 318 | qrtr_publish(perif->data_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_DATA, 0); 319 | qrtr_publish(perif->dci_cmd_fd, DIAG_SERVICE_ID, instance_base + DIAG_INSTANCE_DCI, 0); 320 | 321 | watch_add_readfd(perif->cntl_fd, qrtr_cntl_recv, perif, NULL); 322 | watch_add_readfd(perif->cmd_fd, qrtr_cmd_recv, perif, NULL); 323 | watch_add_readfd(perif->data_fd, qrtr_data_recv, perif, flow); 324 | list_add(&peripherals, &perif->node); 325 | return 0; 326 | } 327 | 328 | int peripheral_qrtr_init(void) 329 | { 330 | qrtr_perif_init_subsystem("modem", DIAG_INSTANCE_BASE_MODEM); 331 | qrtr_perif_init_subsystem("lpass", DIAG_INSTANCE_BASE_LPASS); 332 | qrtr_perif_init_subsystem("wcnss", DIAG_INSTANCE_BASE_WCNSS); 333 | qrtr_perif_init_subsystem("sensors", DIAG_INSTANCE_BASE_SENSORS); 334 | qrtr_perif_init_subsystem("cdsp", DIAG_INSTANCE_BASE_CDSP); 335 | qrtr_perif_init_subsystem("wdsp", DIAG_INSTANCE_BASE_WDSP); 336 | 337 | return 0; 338 | } 339 | -------------------------------------------------------------------------------- /router/peripheral-qrtr.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __PERIPHERAL_QRTR_H__ 32 | #define __PERIPHERAL_QRTR_H__ 33 | 34 | #if HAS_LIBQRTR 35 | int peripheral_qrtr_init(void); 36 | #else 37 | static inline int peripheral_qrtr_init(void) 38 | { 39 | return 0; 40 | } 41 | #endif 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /router/peripheral-rpmsg.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "diag.h" 49 | #include "diag_cntl.h" 50 | #include "dm.h" 51 | #include "hdlc.h" 52 | #include "list.h" 53 | #include "peripheral.h" 54 | #include "util.h" 55 | #include "watch.h" 56 | 57 | #define APPS_BUF_SIZE 16384 58 | 59 | struct devnode { 60 | char *devnode; 61 | char *name; 62 | char *rproc; 63 | 64 | struct list_head node; 65 | }; 66 | 67 | struct list_head devnodes = LIST_INIT(devnodes); 68 | 69 | struct non_hdlc_pkt { 70 | uint8_t start; 71 | uint8_t version; 72 | uint16_t length; 73 | char payload[]; 74 | }; 75 | 76 | static int diag_cmd_recv(int fd, void *data) 77 | { 78 | struct peripheral *peripheral = data; 79 | struct non_hdlc_pkt *frame; 80 | uint8_t buf[APPS_BUF_SIZE]; 81 | ssize_t len; 82 | 83 | len = read(fd, buf, sizeof(buf)); 84 | if (len < 0) { 85 | if (errno != EAGAIN) { 86 | warn("failed to read from cmd channel"); 87 | peripheral_close(peripheral); 88 | } 89 | } 90 | 91 | frame = (struct non_hdlc_pkt *)buf; 92 | if (frame->start != 0x7e || frame->version != 1) { 93 | fprintf(stderr, "invalid non-HDLC frame\n"); 94 | return 0; 95 | } 96 | 97 | if (sizeof(*frame) + frame->length + 1 > len) { 98 | fprintf(stderr, "truncated non-HDLC frame\n"); 99 | return 0; 100 | } 101 | 102 | if (frame->payload[frame->length] != 0x7e) { 103 | fprintf(stderr, "non-HDLC frame is not truncated\n"); 104 | return 0; 105 | } 106 | 107 | dm_broadcast(frame->payload, frame->length, NULL); 108 | 109 | return 0; 110 | } 111 | 112 | static int diag_data_recv_hdlc(int fd, struct peripheral *peripheral) 113 | { 114 | size_t msglen; 115 | ssize_t n; 116 | void *msg; 117 | 118 | for (;;) { 119 | n = circ_read(fd, &peripheral->recv_buf); 120 | if (n < 0) 121 | return -errno; 122 | 123 | for (;;) { 124 | msg = hdlc_decode_one(&peripheral->recv_decoder, 125 | &peripheral->recv_buf, 126 | &msglen); 127 | if (!msg) 128 | break; 129 | 130 | dm_broadcast(msg, msglen, peripheral->flow); 131 | } 132 | } 133 | 134 | /* Not reached */ 135 | } 136 | 137 | static int diag_data_recv_raw(int fd, struct peripheral *peripheral) 138 | { 139 | uint8_t buf[4096]; 140 | ssize_t n; 141 | 142 | for (;;) { 143 | n = read(fd, buf, sizeof(buf)); 144 | if (n < 0) 145 | return -errno; 146 | 147 | dm_broadcast(buf, n, peripheral->flow); 148 | } 149 | 150 | /* Not reached */ 151 | } 152 | 153 | static int diag_data_recv(int fd, void *data) 154 | { 155 | struct peripheral *peripheral = data; 156 | ssize_t n; 157 | 158 | if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE) 159 | n = diag_data_recv_raw(fd, peripheral); 160 | else 161 | n = diag_data_recv_hdlc(fd, peripheral); 162 | 163 | if (n < 0 && n != -EAGAIN) { 164 | warn("failed to read from data channel"); 165 | peripheral_close(peripheral); 166 | } 167 | 168 | return 0; 169 | } 170 | 171 | static int perif_rpmsg_send(struct peripheral *peripheral, const void *ptr, size_t len) 172 | { 173 | struct list_head *queue; 174 | 175 | if (peripheral->features & DIAG_FEATURE_REQ_RSP_SUPPORT) 176 | queue = &peripheral->cmdq; 177 | else 178 | queue = &peripheral->dataq; 179 | 180 | if (peripheral->features & DIAG_FEATURE_APPS_HDLC_ENCODE) 181 | queue_push(queue, ptr, len); 182 | else 183 | hdlc_enqueue(queue, ptr, len); 184 | 185 | return 0; 186 | } 187 | 188 | static struct devnode *devnode_get(const char *devnode) 189 | { 190 | struct list_head *item; 191 | struct devnode *node; 192 | 193 | list_for_each(item, &devnodes) { 194 | node = container_of(item, struct devnode, node); 195 | if (strcmp(node->devnode, devnode) == 0) 196 | return node; 197 | } 198 | 199 | return NULL; 200 | } 201 | 202 | static int devnode_open(const char *rproc, const char *name) 203 | { 204 | struct list_head *item; 205 | struct devnode *node; 206 | 207 | list_for_each(item, &devnodes) { 208 | node = container_of(item, struct devnode, node); 209 | if (strcmp(node->rproc, rproc) == 0 && 210 | strcmp(node->name, name) == 0) 211 | return open(node->devnode, O_RDWR); 212 | } 213 | 214 | return -1; 215 | } 216 | 217 | static void devnode_add(const char *devnode, const char *name, const char *rproc) 218 | { 219 | struct devnode *node; 220 | 221 | node = devnode_get(devnode); 222 | if (node) { 223 | warnx("node already in list"); 224 | return; 225 | } 226 | 227 | node = malloc(sizeof(*node)); 228 | memset(node, 0, sizeof(*node)); 229 | 230 | node->devnode = strdup(devnode); 231 | node->name = strdup(name); 232 | node->rproc = strdup(rproc); 233 | 234 | list_add(&devnodes, &node->node); 235 | } 236 | 237 | static void devnode_remove(const char *devnode) 238 | { 239 | struct devnode *node; 240 | 241 | node = devnode_get(devnode); 242 | if (!node) 243 | return; 244 | 245 | list_del(&node->node); 246 | 247 | free(node->name); 248 | free(node->devnode); 249 | free(node->rproc); 250 | } 251 | 252 | static const char *peripheral_udev_get_name(struct udev_device *dev) 253 | { 254 | return udev_device_get_sysattr_value(dev, "name"); 255 | } 256 | 257 | static const char *peripheral_udev_get_remoteproc(struct udev_device *dev) 258 | { 259 | struct udev_device *parent; 260 | const char *p; 261 | 262 | parent = udev_device_get_parent(dev); 263 | if (!parent) 264 | return NULL; 265 | 266 | p = udev_device_get_sysattr_value(parent, "rpmsg_name"); 267 | if (p) 268 | return p; 269 | 270 | return peripheral_udev_get_remoteproc(parent); 271 | } 272 | 273 | static int rpmsg_perif_cntl_recv(int fd, void *data) 274 | { 275 | struct peripheral *peripheral = data; 276 | uint8_t buf[4096]; 277 | ssize_t n; 278 | 279 | n = read(fd, buf, sizeof(buf)); 280 | if (n < 0) { 281 | if (errno != EAGAIN) { 282 | warn("failed to read from cntl channel"); 283 | peripheral_close(peripheral); 284 | } 285 | return 0; 286 | } 287 | 288 | return diag_cntl_recv(peripheral, buf, n); 289 | } 290 | 291 | static void peripheral_open(void *data) 292 | { 293 | struct peripheral *peripheral = data; 294 | char *rproc = peripheral->name; 295 | int ret; 296 | int fd; 297 | 298 | fd = devnode_open(rproc, "DIAG"); 299 | if (fd < 0) 300 | fd = devnode_open(rproc, "APPS_RIVA_DATA"); 301 | if (fd < 0) { 302 | warn("unable to open DIAG channel"); 303 | return; 304 | } 305 | peripheral->data_fd = fd; 306 | 307 | fd = devnode_open(rproc, "DIAG_CNTL"); 308 | if (fd < 0) 309 | fd = devnode_open(rproc, "APPS_RIVA_CTRL"); 310 | if (fd < 0) { 311 | warn("unable to find DIAG_CNTL channel"); 312 | close(peripheral->data_fd); 313 | peripheral->data_fd = -1; 314 | return; 315 | } 316 | peripheral->cntl_fd = fd; 317 | 318 | fd = devnode_open(rproc, "DIAG_CMD"); 319 | if (fd >= 0) 320 | peripheral->cmd_fd = fd; 321 | 322 | ret = fcntl(peripheral->data_fd, F_SETFL, O_NONBLOCK); 323 | if (ret < 0) 324 | warn("failed to turn DIAG non blocking"); 325 | 326 | watch_add_writeq(peripheral->cntl_fd, &peripheral->cntlq); 327 | watch_add_writeq(peripheral->data_fd, &peripheral->dataq); 328 | watch_add_readfd(peripheral->cntl_fd, rpmsg_perif_cntl_recv, peripheral, NULL); 329 | watch_add_readfd(peripheral->data_fd, diag_data_recv, peripheral, peripheral->flow); 330 | if (peripheral->cmd_fd >= 0) { 331 | watch_add_readfd(peripheral->cmd_fd, diag_cmd_recv, peripheral, NULL); 332 | watch_add_writeq(peripheral->cmd_fd, &peripheral->cmdq); 333 | } 334 | 335 | /* Send current message mask to the newly found peripheral */ 336 | diag_cntl_send_masks(peripheral); 337 | } 338 | 339 | static void perif_rpmsg_close(struct peripheral *peripheral) 340 | { 341 | diag_cntl_close(peripheral); 342 | 343 | watch_remove_fd(peripheral->data_fd); 344 | watch_remove_fd(peripheral->cntl_fd); 345 | watch_remove_fd(peripheral->cmd_fd); 346 | 347 | close(peripheral->data_fd); 348 | close(peripheral->cntl_fd); 349 | close(peripheral->cmd_fd); 350 | 351 | list_del(&peripheral->node); 352 | free(peripheral->name); 353 | free(peripheral); 354 | } 355 | 356 | static int peripheral_create(const char *rproc, const char *channel) 357 | { 358 | struct peripheral *peripheral; 359 | struct watch_flow *flow; 360 | struct list_head *item; 361 | 362 | /* Only trigger the creation of a peripheral on primary channels */ 363 | if (strcmp(channel, "DIAG") && strcmp(channel, "APPS_RIVA_DATA")) 364 | return 0; 365 | 366 | list_for_each(item, &peripherals) { 367 | peripheral = container_of(item, struct peripheral, node); 368 | if (strcmp(peripheral->name, rproc) == 0) 369 | return 0; 370 | } 371 | 372 | peripheral = malloc(sizeof(*peripheral)); 373 | memset(peripheral, 0, sizeof(*peripheral)); 374 | 375 | flow = watch_flow_new(); 376 | 377 | peripheral->name = strdup(rproc); 378 | peripheral->data_fd = -1; 379 | peripheral->cntl_fd = -1; 380 | peripheral->cmd_fd = -1; 381 | peripheral->send = perif_rpmsg_send; 382 | peripheral->close = perif_rpmsg_close; 383 | peripheral->flow = flow; 384 | list_init(&peripheral->cmdq); 385 | list_init(&peripheral->cntlq); 386 | list_init(&peripheral->dataq); 387 | list_add(&peripherals, &peripheral->node); 388 | 389 | watch_add_timer(peripheral_open, peripheral, 1000, false); 390 | 391 | return 0; 392 | } 393 | 394 | static int peripheral_udev_update(int fd, void *data) 395 | { 396 | struct udev_monitor *mon = data; 397 | struct udev_device *dev; 398 | const char *devnode; 399 | const char *action; 400 | const char *rproc; 401 | const char *name; 402 | 403 | dev = udev_monitor_receive_device(mon); 404 | if (!dev) 405 | return 0; 406 | 407 | action = udev_device_get_action(dev); 408 | devnode = udev_device_get_devnode(dev); 409 | 410 | if (!devnode) 411 | goto unref_dev; 412 | 413 | if (strcmp(action, "add") == 0) { 414 | name = peripheral_udev_get_name(dev); 415 | rproc = peripheral_udev_get_remoteproc(dev); 416 | 417 | if (!name || !rproc) 418 | goto unref_dev; 419 | 420 | devnode_add(devnode, name, rproc); 421 | 422 | peripheral_create(rproc, name); 423 | } else if (strcmp(action, "remove") == 0) { 424 | devnode_remove(devnode); 425 | } else { 426 | warn("unknown udev action"); 427 | } 428 | 429 | unref_dev: 430 | udev_device_unref(dev); 431 | 432 | return 0; 433 | } 434 | 435 | int peripheral_rpmsg_init(void) 436 | { 437 | struct udev_list_entry *devices; 438 | struct udev_list_entry *entry; 439 | struct udev_enumerate *enu; 440 | struct udev_monitor *mon; 441 | struct udev_device *dev; 442 | struct udev *udev; 443 | const char *devnode; 444 | const char *path; 445 | const char *rproc; 446 | const char *name; 447 | int fd; 448 | 449 | udev = udev_new(); 450 | if (!udev) 451 | err(1, "failed to initialize libudev"); 452 | 453 | mon = udev_monitor_new_from_netlink(udev, "udev"); 454 | udev_monitor_filter_add_match_subsystem_devtype(mon, "rpmsg", NULL); 455 | udev_monitor_enable_receiving(mon); 456 | 457 | fd = udev_monitor_get_fd(mon); 458 | 459 | enu = udev_enumerate_new(udev); 460 | udev_enumerate_add_match_subsystem(enu, "rpmsg"); 461 | udev_enumerate_scan_devices(enu); 462 | 463 | devices = udev_enumerate_get_list_entry(enu); 464 | udev_list_entry_foreach(entry, devices) { 465 | path = udev_list_entry_get_name(entry); 466 | dev = udev_device_new_from_syspath(udev, path); 467 | 468 | devnode = udev_device_get_devnode(dev); 469 | name = peripheral_udev_get_name(dev); 470 | rproc = peripheral_udev_get_remoteproc(dev); 471 | 472 | if (devnode && name && rproc) { 473 | devnode_add(devnode, name, rproc); 474 | peripheral_create(rproc, name); 475 | } 476 | 477 | udev_device_unref(dev); 478 | } 479 | 480 | watch_add_readfd(fd, peripheral_udev_update, mon, NULL); 481 | 482 | return 0; 483 | } 484 | -------------------------------------------------------------------------------- /router/peripheral-rpmsg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __PERIPHERAL_RPMSG_H__ 32 | #define __PERIPHERAL_RPMSG_H__ 33 | 34 | #if HAS_LIBUDEV 35 | int peripheral_rpmsg_init(void); 36 | #else 37 | static inline int peripheral_rpmsg_init(void) 38 | { 39 | return 0; 40 | } 41 | #endif 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /router/peripheral.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "diag.h" 42 | #include "diag_cntl.h" 43 | #include "dm.h" 44 | #include "hdlc.h" 45 | #include "list.h" 46 | #include "peripheral.h" 47 | #include "peripheral-qrtr.h" 48 | #include "peripheral-rpmsg.h" 49 | #include "util.h" 50 | #include "watch.h" 51 | 52 | struct list_head peripherals = LIST_INIT(peripherals); 53 | 54 | int peripheral_send(struct peripheral *peripheral, const void *ptr, size_t len) 55 | { 56 | return peripheral->send(peripheral, ptr, len); 57 | } 58 | 59 | void peripheral_close(struct peripheral *peripheral) 60 | { 61 | peripheral->close(peripheral); 62 | } 63 | 64 | int peripheral_init(void) 65 | { 66 | peripheral_rpmsg_init(); 67 | peripheral_qrtr_init(); 68 | 69 | return 0; 70 | } 71 | 72 | void peripheral_broadcast_event_mask(void) 73 | { 74 | struct peripheral *peripheral; 75 | struct list_head *item; 76 | 77 | list_for_each(item, &peripherals) { 78 | peripheral = container_of(item, struct peripheral, node); 79 | 80 | diag_cntl_send_event_mask(peripheral); 81 | } 82 | } 83 | 84 | void peripheral_broadcast_log_mask(unsigned int equip_id) 85 | { 86 | struct peripheral *peripheral; 87 | struct list_head *item; 88 | 89 | list_for_each(item, &peripherals) { 90 | peripheral = container_of(item, struct peripheral, node); 91 | 92 | diag_cntl_send_log_mask(peripheral, equip_id); 93 | } 94 | } 95 | 96 | void peripheral_broadcast_msg_mask(struct diag_ssid_range_t *range) 97 | { 98 | struct peripheral *peripheral; 99 | struct list_head *item; 100 | 101 | list_for_each(item, &peripherals) { 102 | peripheral = container_of(item, struct peripheral, node); 103 | 104 | diag_cntl_send_msg_mask(peripheral, range); 105 | } 106 | } 107 | -------------------------------------------------------------------------------- /router/peripheral.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __PERIPHERAL_H__ 32 | #define __PERIPHERAL_H__ 33 | 34 | struct diag_ssid_range_t; 35 | 36 | int peripheral_init(void); 37 | void peripheral_close(struct peripheral *peripheral); 38 | 39 | void peripheral_broadcast_event_mask(void); 40 | void peripheral_broadcast_log_mask(unsigned int equip_id); 41 | void peripheral_broadcast_msg_mask(struct diag_ssid_range_t *range); 42 | 43 | int peripheral_send(struct peripheral *peripheral, const void *ptr, size_t len); 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /router/router.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2018, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "diag.h" 39 | #include "dm.h" 40 | #include "hdlc.h" 41 | #include "peripheral.h" 42 | #include "util.h" 43 | 44 | #define DIAG_CMD_RSP_BAD_COMMAND 0x13 45 | #define DIAG_CMD_RSP_BAD_PARAMS 0x14 46 | #define DIAG_CMD_RSP_BAD_LENGTH 0x15 47 | 48 | struct list_head fallback_cmds = LIST_INIT(fallback_cmds); 49 | struct list_head common_cmds = LIST_INIT(common_cmds); 50 | 51 | int hdlc_enqueue_flow(struct list_head *queue, const void *msg, size_t msglen, 52 | struct watch_flow *flow) 53 | { 54 | size_t outlen; 55 | void *outbuf; 56 | 57 | outbuf = hdlc_encode(msg, msglen, &outlen); 58 | if (!outbuf) 59 | err(1, "failed to allocate hdlc destination buffer"); 60 | 61 | queue_push_flow(queue, outbuf, outlen, flow); 62 | free(outbuf); 63 | 64 | return 0; 65 | } 66 | 67 | int hdlc_enqueue(struct list_head *queue, const void *msg, size_t msglen) 68 | { 69 | return hdlc_enqueue_flow(queue, msg, msglen, NULL); 70 | } 71 | 72 | static int diag_cmd_dispatch(struct diag_client *client, uint8_t *ptr, 73 | size_t len) 74 | { 75 | struct list_head *item; 76 | struct diag_cmd *dc; 77 | unsigned int key; 78 | int handled = 0; 79 | 80 | if (ptr[0] == DIAG_CMD_SUBSYS_DISPATCH || 81 | ptr[0] == DIAG_CMD_SUBSYS_DISPATCH_V2) 82 | key = ptr[0] << 24 | ptr[1] << 16 | ptr[3] << 8 | ptr[2]; 83 | else 84 | key = 0xff << 24 | 0xff << 16 | ptr[0]; 85 | 86 | list_for_each(item, &common_cmds) { 87 | dc = container_of(item, struct diag_cmd, node); 88 | if (key < dc->first || key > dc->last) 89 | continue; 90 | 91 | return dc->cb(client, ptr, len); 92 | } 93 | 94 | list_for_each(item, &diag_cmds) { 95 | dc = container_of(item, struct diag_cmd, node); 96 | if (key < dc->first || key > dc->last) 97 | continue; 98 | 99 | if (dc->cb) 100 | dc->cb(client, ptr, len); 101 | else 102 | peripheral_send(dc->peripheral, ptr, len); 103 | 104 | handled++; 105 | } 106 | 107 | if (handled) 108 | return 0; 109 | 110 | list_for_each_entry(dc, &fallback_cmds, node) { 111 | if (key < dc->first || key > dc->last) 112 | continue; 113 | 114 | return dc->cb(client, ptr, len); 115 | } 116 | 117 | return -ENOENT; 118 | } 119 | 120 | static void diag_rsp_bad_command(struct diag_client *client, uint8_t *msg, 121 | size_t len, int error_code) 122 | { 123 | uint8_t *buf; 124 | 125 | buf = malloc(len + 1); 126 | if (!buf) 127 | err(1, "failed to allocate error buffer"); 128 | 129 | buf[0] = error_code; 130 | memcpy(buf + 1, msg, len); 131 | 132 | dm_send(client, buf, len + 1); 133 | 134 | free(buf); 135 | } 136 | 137 | int diag_client_handle_command(struct diag_client *client, uint8_t *data, size_t len) 138 | { 139 | int ret; 140 | 141 | ret = diag_cmd_dispatch(client, data, len); 142 | 143 | switch (ret) { 144 | case -ENOENT: 145 | diag_rsp_bad_command(client, data, len, DIAG_CMD_RSP_BAD_COMMAND); 146 | break; 147 | case -EINVAL: 148 | diag_rsp_bad_command(client, data, len, DIAG_CMD_RSP_BAD_PARAMS); 149 | break; 150 | case -EMSGSIZE: 151 | diag_rsp_bad_command(client, data, len, DIAG_CMD_RSP_BAD_LENGTH); 152 | break; 153 | default: 154 | break; 155 | } 156 | 157 | return 0; 158 | } 159 | 160 | void register_fallback_cmd(unsigned int cmd, 161 | int(*cb)(struct diag_client *client, 162 | const void *buf, size_t len)) 163 | { 164 | struct diag_cmd *dc; 165 | unsigned int key = 0xffff0000 | cmd; 166 | 167 | dc = calloc(1, sizeof(*dc)); 168 | if (!dc) 169 | err(1, "failed to allocate diag command\n"); 170 | 171 | dc->first = key; 172 | dc->last = key; 173 | dc->cb = cb; 174 | 175 | list_add(&fallback_cmds, &dc->node); 176 | } 177 | 178 | void register_fallback_subsys_cmd(unsigned int subsys, unsigned int cmd, 179 | int(*cb)(struct diag_client *client, 180 | const void *buf, size_t len)) 181 | { 182 | struct diag_cmd *dc; 183 | unsigned int key = DIAG_CMD_SUBSYS_DISPATCH << 24 | 184 | (subsys & 0xff) << 16 | cmd; 185 | 186 | dc = calloc(1, sizeof(*dc)); 187 | if (!dc) 188 | err(1, "failed to allocate diag command\n"); 189 | 190 | dc->first = key; 191 | dc->last = key; 192 | dc->cb = cb; 193 | 194 | list_add(&fallback_cmds, &dc->node); 195 | } 196 | 197 | void register_common_cmd(unsigned int cmd, int(*cb)(struct diag_client *client, 198 | const void *buf, 199 | size_t len)) 200 | { 201 | struct diag_cmd *dc; 202 | unsigned int key = 0xffff0000 | cmd; 203 | 204 | dc = calloc(1, sizeof(*dc)); 205 | if (!dc) 206 | err(1, "failed to allocate diag command\n"); 207 | 208 | dc->first = key; 209 | dc->last = key; 210 | dc->cb = cb; 211 | 212 | list_add(&common_cmds, &dc->node); 213 | } 214 | -------------------------------------------------------------------------------- /router/socket.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * Copyright (c) 2016, Linaro Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * 3. Neither the name of the copyright holder nor the names of its contributors 17 | * may be used to endorse or promote products derived from this software without 18 | * specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 24 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 25 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 26 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 27 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 28 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 29 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "diag.h" 47 | #include "dm.h" 48 | #include "hdlc.h" 49 | #include "watch.h" 50 | 51 | #define APPS_BUF_SIZE 16384 52 | 53 | int diag_sock_connect(const char *hostname, unsigned short port) 54 | { 55 | struct sockaddr_in addr; 56 | struct diag_client *dm; 57 | struct hostent *host; 58 | int ret; 59 | int fd; 60 | 61 | fd = socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0); 62 | if (fd < 0) 63 | return -errno; 64 | 65 | host = gethostbyname(hostname); 66 | if (!host) 67 | return -errno; 68 | 69 | bzero(&addr, sizeof(addr)); 70 | addr.sin_family = AF_INET; 71 | bcopy(host->h_addr, &addr.sin_addr.s_addr, host->h_length); 72 | addr.sin_port = htons(port); 73 | 74 | ret = connect(fd, (const struct sockaddr *)&addr, sizeof(addr)); 75 | if (ret < 0) 76 | return -errno; 77 | 78 | ret = fcntl(fd, F_SETFL, O_NONBLOCK); 79 | if (ret < 0) 80 | return -errno; 81 | 82 | printf("Connected to %s:%d\n", hostname, port); 83 | 84 | dm = dm_add("DIAG CLIENT", fd, fd, true); 85 | dm_enable(dm); 86 | 87 | return fd; 88 | } 89 | -------------------------------------------------------------------------------- /router/uart.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution. 13 | * * Neither the name of The Linux Foundation nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 18 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 21 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 27 | * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "diag.h" 40 | #include "dm.h" 41 | #include "hdlc.h" 42 | #include "watch.h" 43 | 44 | #define APPS_BUF_SIZE 16384 45 | 46 | static unsigned int check_baudrate(unsigned int baudrate) 47 | { 48 | switch (baudrate) 49 | { 50 | case 9600: 51 | return B9600; 52 | case 19200: 53 | return B19200; 54 | case 38400: 55 | return B38400; 56 | case 115200: 57 | return B115200; 58 | default: 59 | warn("Illegal baud rate %u!", baudrate); 60 | return 0; 61 | } 62 | } 63 | 64 | int diag_uart_open(const char *uartname, unsigned int baudrate) 65 | { 66 | struct diag_client *dm; 67 | int ret; 68 | int fd; 69 | struct termios options, options_save; 70 | 71 | baudrate = check_baudrate(baudrate); 72 | if (baudrate == 0) 73 | return -EINVAL; 74 | 75 | fd = open(uartname, O_RDWR | O_NOCTTY | O_NONBLOCK); 76 | if (fd < 0) 77 | return -errno; 78 | 79 | ret = tcflush(fd, TCIOFLUSH); 80 | if (ret < 0) 81 | return -errno; 82 | 83 | ret = fcntl(fd, F_SETFL, 0); 84 | if (ret < 0) 85 | return -errno; 86 | 87 | ret = ioctl(fd, TCGETS, &options_save); 88 | if (ret < 0) 89 | return -errno; 90 | 91 | options = options_save; 92 | options.c_cc[VTIME] = 0; /* inter-character timer unused */ 93 | options.c_cc[VMIN] = 4; /* blocking read until 4 chars received */ 94 | options.c_cflag &= ~PARENB; 95 | options.c_cflag &= ~CSTOPB; 96 | options.c_cflag &= ~CSIZE; 97 | options.c_cflag |= (CS8 | CLOCAL | CREAD); 98 | options.c_iflag = 0; 99 | options.c_oflag = 0; 100 | options.c_lflag = 0; 101 | options.c_cflag = (options.c_cflag & ~CBAUD) | (baudrate & CBAUD); 102 | 103 | ret = ioctl(fd, TCSETS, &options); // TODO: need to call ioctl(ret, TCSETS, &options_save) to revert to original state 104 | if (ret < 0) 105 | return -errno; 106 | 107 | printf("Connected to %s@%d\n", uartname, baudrate); 108 | 109 | dm = dm_add("UART client", fd, fd, true); 110 | dm_enable(dm); 111 | 112 | return fd; 113 | } 114 | -------------------------------------------------------------------------------- /router/unix.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | #include "diag.h" 40 | #include "dm.h" 41 | #include "watch.h" 42 | 43 | static int unix_listen(int fd, void *data) 44 | { 45 | struct diag_client *dm; 46 | int client; 47 | int ret; 48 | 49 | client = accept(fd, NULL, NULL); 50 | if (client < 0) { 51 | fprintf(stderr, "failed to accept"); 52 | return 0; 53 | } 54 | 55 | ret = fcntl(client, F_SETFL, O_NONBLOCK); 56 | if (ret < 0) { 57 | fprintf(stderr, "failed to set O_NONBLOCK"); 58 | return 0; 59 | } 60 | 61 | dm = dm_add("UNIX", client, client, false); 62 | dm_enable(dm); 63 | 64 | return 0; 65 | } 66 | 67 | int diag_unix_open(void) 68 | { 69 | struct sockaddr_un addr; 70 | int ret; 71 | int fd; 72 | 73 | fd = socket(AF_UNIX, SOCK_SEQPACKET, 0); 74 | if (fd < 0) { 75 | fprintf(stderr, "failed to create unix socket"); 76 | return -1; 77 | } 78 | 79 | memset(&addr, 0, sizeof(addr)); 80 | addr.sun_family = AF_UNIX; 81 | strncpy(addr.sun_path, "\0diag", sizeof(addr.sun_path)-1); 82 | ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr)); 83 | if (ret < 0) { 84 | fprintf(stderr, "failed to bind diag socket"); 85 | return -1; 86 | } 87 | 88 | ret = listen(fd, 2); 89 | if (ret < 0) { 90 | fprintf(stderr, "failed to listen on diag socket\n"); 91 | return -1; 92 | } 93 | 94 | watch_add_readfd(fd, unix_listen, NULL, NULL); 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /router/usb.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016-2017, The Linux Foundation. All rights reserved. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution. 13 | * * Neither the name of The Linux Foundation nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED 18 | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 19 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS 21 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 24 | * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 25 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN 27 | * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #define _DEFAULT_SOURCE /* for endian.h */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include "diag.h" 50 | #include "dm.h" 51 | #include "mbuf.h" 52 | #include "hdlc.h" 53 | #include "util.h" 54 | #include "watch.h" 55 | 56 | #define USB_FFS_EP0_NAME "ep0" 57 | #define USB_FFS_OUT_NAME "ep1" 58 | #define USB_FFS_IN_NAME "ep2" 59 | 60 | #define USB_PROTOCOL_DIAG 0x30 61 | 62 | #if __BYTE_ORDER == __LITTLE_ENDIAN 63 | #define cpu_to_le16(x) (x) 64 | #define cpu_to_le32(x) (x) 65 | #else 66 | #define cpu_to_le16(x) ((((x) >> 8) & 0xffu) | (((x) & 0xffu) << 8)) 67 | #define cpu_to_le32(x) ((((x) & 0xff000000u) >> 24) | \ 68 | (((x) & 0x00ff0000u) >> 8) | \ 69 | (((x) & 0x0000ff00u) << 8) | \ 70 | (((x) & 0x000000ffu) << 24)) 71 | #endif 72 | 73 | static const struct { 74 | struct usb_functionfs_descs_head_v2 header; 75 | __le32 fs_count; 76 | __le32 hs_count; 77 | __le32 ss_count; 78 | struct { 79 | struct usb_interface_descriptor intf; 80 | struct usb_endpoint_descriptor_no_audio source; 81 | struct usb_endpoint_descriptor_no_audio sink; 82 | } __packed fs_descs, hs_descs; 83 | 84 | struct { 85 | struct usb_interface_descriptor intf; 86 | struct usb_endpoint_descriptor_no_audio source; 87 | struct usb_ss_ep_comp_descriptor source_comp; 88 | struct usb_endpoint_descriptor_no_audio sink; 89 | struct usb_ss_ep_comp_descriptor sink_comp; 90 | } __packed ss_descs; 91 | } __packed descriptors = { 92 | .header = { 93 | .magic = cpu_to_le32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2), 94 | .flags = cpu_to_le32(FUNCTIONFS_HAS_FS_DESC | 95 | FUNCTIONFS_HAS_HS_DESC | 96 | FUNCTIONFS_HAS_SS_DESC), 97 | .length = cpu_to_le32(sizeof(descriptors)), 98 | }, 99 | .fs_count = cpu_to_le32(3), 100 | .fs_descs = { 101 | .intf = { 102 | .bLength = sizeof(descriptors.fs_descs.intf), 103 | .bDescriptorType = USB_DT_INTERFACE, 104 | .bNumEndpoints = 2, 105 | .bInterfaceClass = USB_CLASS_VENDOR_SPEC, 106 | .bInterfaceSubClass = USB_SUBCLASS_VENDOR_SPEC, 107 | .bInterfaceProtocol = USB_PROTOCOL_DIAG, 108 | .iInterface = 1, 109 | }, 110 | .source = { 111 | .bLength = sizeof(descriptors.fs_descs.source), 112 | .bDescriptorType = USB_DT_ENDPOINT, 113 | .bEndpointAddress = 1 | USB_DIR_OUT, 114 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 115 | .wMaxPacketSize = cpu_to_le16(64), 116 | }, 117 | .sink = { 118 | .bLength = sizeof(descriptors.fs_descs.sink), 119 | .bDescriptorType = USB_DT_ENDPOINT, 120 | .bEndpointAddress = 2 | USB_DIR_IN, 121 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 122 | .wMaxPacketSize = cpu_to_le16(64), 123 | }, 124 | }, 125 | .hs_count = cpu_to_le32(3), 126 | .hs_descs = { 127 | .intf = { 128 | .bLength = sizeof(descriptors.hs_descs.intf), 129 | .bDescriptorType = USB_DT_INTERFACE, 130 | .bNumEndpoints = 2, 131 | .bInterfaceClass = USB_CLASS_VENDOR_SPEC, 132 | .bInterfaceSubClass = USB_SUBCLASS_VENDOR_SPEC, 133 | .bInterfaceProtocol = USB_PROTOCOL_DIAG, 134 | .iInterface = 1, 135 | }, 136 | .source = { 137 | .bLength = sizeof(descriptors.hs_descs.source), 138 | .bDescriptorType = USB_DT_ENDPOINT, 139 | .bEndpointAddress = 1 | USB_DIR_OUT, 140 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 141 | .wMaxPacketSize = cpu_to_le16(512), 142 | }, 143 | .sink = { 144 | .bLength = sizeof(descriptors.hs_descs.sink), 145 | .bDescriptorType = USB_DT_ENDPOINT, 146 | .bEndpointAddress = 2 | USB_DIR_IN, 147 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 148 | .wMaxPacketSize = cpu_to_le16(512), 149 | }, 150 | }, 151 | .ss_count = cpu_to_le32(5), 152 | .ss_descs = { 153 | .intf = { 154 | .bLength = sizeof(descriptors.ss_descs.intf), 155 | .bDescriptorType = USB_DT_INTERFACE, 156 | .bNumEndpoints = 2, 157 | .bInterfaceClass = USB_CLASS_VENDOR_SPEC, 158 | .bInterfaceSubClass = USB_SUBCLASS_VENDOR_SPEC, 159 | .bInterfaceProtocol = 0x30, 160 | .iInterface = 1, 161 | }, 162 | .source = { 163 | .bLength = sizeof(descriptors.ss_descs.source), 164 | .bDescriptorType = USB_DT_ENDPOINT, 165 | .bEndpointAddress = 1 | USB_DIR_OUT, 166 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 167 | .wMaxPacketSize = cpu_to_le16(1024), 168 | }, 169 | .source_comp = { 170 | .bLength = sizeof(descriptors.ss_descs.source_comp), 171 | .bDescriptorType = USB_DT_SS_ENDPOINT_COMP, 172 | }, 173 | .sink = { 174 | .bLength = sizeof(descriptors.ss_descs.sink), 175 | .bDescriptorType = USB_DT_ENDPOINT, 176 | .bEndpointAddress = 2 | USB_DIR_IN, 177 | .bmAttributes = USB_ENDPOINT_XFER_BULK, 178 | .wMaxPacketSize = cpu_to_le16(1024), 179 | }, 180 | .sink_comp = { 181 | .bLength = sizeof(descriptors.ss_descs.sink_comp), 182 | .bDescriptorType = USB_DT_SS_ENDPOINT_COMP, 183 | }, 184 | }, 185 | }; 186 | 187 | #define STR_INTERFACE_ "Diag interface" 188 | 189 | static const struct { 190 | struct usb_functionfs_strings_head header; 191 | struct { 192 | __le16 code; 193 | const char str1[sizeof(STR_INTERFACE_)]; 194 | } __attribute__((packed)) lang0; 195 | } __attribute__((packed)) strings = { 196 | .header = { 197 | .magic = cpu_to_le32(FUNCTIONFS_STRINGS_MAGIC), 198 | .length = cpu_to_le32(sizeof(strings)), 199 | .str_count = cpu_to_le32(1), 200 | .lang_count = cpu_to_le32(1), 201 | }, 202 | .lang0 = { 203 | cpu_to_le16(0x0409), /* en-us */ 204 | STR_INTERFACE_, 205 | }, 206 | }; 207 | 208 | struct usb_handle { 209 | int ep0; 210 | int bulk_out; /* "out" from the host's perspective => source for diagd */ 211 | int bulk_in; /* "in" from the host's perspective => sink for diagd */ 212 | 213 | struct diag_client *dm; 214 | struct list_head outq; 215 | }; 216 | 217 | static int ffs_diag_init(const char *ffs_name, struct usb_handle *h) 218 | { 219 | int ffs_fd; 220 | ssize_t n; 221 | int ret; 222 | 223 | ret = open(ffs_name, O_DIRECTORY); 224 | if (ret < 0) { 225 | warn("cannot open device folder %s", ffs_name); 226 | goto err_out; 227 | } 228 | ffs_fd = ret; 229 | 230 | ret = openat(ffs_fd, USB_FFS_EP0_NAME, O_RDWR); 231 | if (ret < 0) { 232 | warn("cannot open control endpoint"); 233 | goto err_close_ffs; 234 | } 235 | h->ep0 = ret; 236 | 237 | n = write(h->ep0, &descriptors, sizeof(descriptors)); 238 | if (n < 0) { 239 | warn("failed to write descriptors"); 240 | ret = n; 241 | goto err_close_ep0; 242 | } 243 | 244 | n = write(h->ep0, &strings, sizeof(strings)); 245 | if (n < 0) { 246 | warn("failed to write strings"); 247 | ret = n; 248 | goto err_close_ep0; 249 | } 250 | 251 | ret = openat(ffs_fd, USB_FFS_OUT_NAME, O_RDWR | O_NOCTTY | O_NONBLOCK); 252 | if (ret < 0) { 253 | warn("cannot open bulk-out ep"); 254 | goto err_close_ep0; 255 | } 256 | h->bulk_out = ret; 257 | 258 | ret = openat(ffs_fd, USB_FFS_IN_NAME, O_RDWR); 259 | if (ret < 0) { 260 | warn("cannot open bulk-in ep"); 261 | goto err_close_bulk_out; 262 | } 263 | h->bulk_in = ret; 264 | 265 | close(ffs_fd); 266 | 267 | return 0; 268 | 269 | err_close_bulk_out: 270 | close(h->bulk_out); 271 | err_close_ep0: 272 | close(h->ep0); 273 | err_close_ffs: 274 | close(ffs_fd); 275 | err_out: 276 | return ret; 277 | } 278 | 279 | static int diag_ffs_recv(struct mbuf *mbuf, void *data) 280 | { 281 | struct hdlc_decoder recv_decoder; 282 | struct circ_buf recv_buf; 283 | struct usb_handle *ffs = data; 284 | size_t msglen; 285 | void *msg; 286 | 287 | memset(&recv_decoder, 0, sizeof(recv_decoder)); 288 | 289 | memcpy(recv_buf.buf, mbuf->data, mbuf->offset); 290 | recv_buf.tail = 0; 291 | recv_buf.head = mbuf->offset; 292 | 293 | // print_hex_dump("[USB]", mbuf->data, mbuf->offset); 294 | 295 | for (;;) { 296 | msg = hdlc_decode_one(&recv_decoder, &recv_buf, &msglen); 297 | if (!msg) 298 | break; 299 | 300 | // print_hex_dump(" [MSG]", msg, MIN(msglen, 256)); 301 | 302 | diag_client_handle_command(ffs->dm, msg, msglen); 303 | } 304 | 305 | mbuf->offset = 0; 306 | list_add(&ffs->outq, &mbuf->node); 307 | 308 | return 0; 309 | } 310 | 311 | static int ep0_recv(int fd, void *data) 312 | { 313 | struct usb_functionfs_event event; 314 | struct usb_handle *ffs = data; 315 | ssize_t n; 316 | 317 | n = read(fd, &event, sizeof(event)); 318 | if (n <= 0) { 319 | warn("failed to read ffs ep0"); 320 | return 0; 321 | } 322 | 323 | switch (event.type) { 324 | case FUNCTIONFS_ENABLE: 325 | watch_add_readq(ffs->bulk_out, &ffs->outq, diag_ffs_recv, ffs); 326 | dm_enable(ffs->dm); 327 | break; 328 | case FUNCTIONFS_DISABLE: 329 | dm_disable(ffs->dm); 330 | break; 331 | } 332 | 333 | return 0; 334 | } 335 | 336 | int diag_usb_open(const char *ffs_name) 337 | { 338 | struct usb_handle *ffs; 339 | struct mbuf *out_buf; 340 | int ret; 341 | 342 | ffs = calloc(1, sizeof(struct usb_handle)); 343 | if (!ffs) 344 | err(1, "couldn't allocate usb_handle"); 345 | 346 | out_buf = mbuf_alloc(16384); 347 | if (!out_buf) 348 | err(1, "couldn't allocate usb out buffer"); 349 | 350 | ret = ffs_diag_init(ffs_name, ffs); 351 | if (ret < 0) { 352 | free(ffs); 353 | return -1; 354 | } 355 | 356 | list_init(&ffs->outq); 357 | list_add(&ffs->outq, &out_buf->node); 358 | 359 | watch_add_readfd(ffs->ep0, ep0_recv, ffs, NULL); 360 | 361 | ffs->dm = dm_add("USB client", -1, ffs->bulk_in, true); 362 | 363 | return 0; 364 | } 365 | -------------------------------------------------------------------------------- /router/util.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Bjorn Andersson 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #include 32 | #include 33 | #include 34 | #include "util.h" 35 | 36 | static uint8_t to_hex(uint8_t ch) 37 | { 38 | ch &= 0xf; 39 | return ch <= 9 ? '0' + ch : 'a' + ch - 10; 40 | } 41 | 42 | void print_hex_dump(const char *prefix, const void *buf, size_t len) 43 | { 44 | const uint8_t *ptr = buf; 45 | size_t linelen; 46 | uint8_t ch; 47 | char line[16 * 3 + 16 + 1]; 48 | int li; 49 | int i; 50 | int j; 51 | 52 | for (i = 0; i < len; i += 16) { 53 | linelen = MIN(16, len - i); 54 | li = 0; 55 | 56 | for (j = 0; j < linelen; j++) { 57 | ch = ptr[i + j]; 58 | line[li++] = to_hex(ch >> 4); 59 | line[li++] = to_hex(ch); 60 | line[li++] = ' '; 61 | } 62 | 63 | for (; j < 16; j++) { 64 | line[li++] = ' '; 65 | line[li++] = ' '; 66 | line[li++] = ' '; 67 | } 68 | 69 | for (j = 0; j < linelen; j++) { 70 | ch = ptr[i + j]; 71 | line[li++] = isprint(ch) ? ch : '.'; 72 | } 73 | 74 | line[li] = '\0'; 75 | 76 | printf("%s %04x: %s\n", prefix, i, line); 77 | } 78 | } 79 | -------------------------------------------------------------------------------- /router/util.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __UTIL_H__ 32 | #define __UTIL_H__ 33 | 34 | #define ARRAY_SIZE(x) (sizeof(x)/sizeof((x)[0])) 35 | 36 | #define MIN(x, y) ((x) < (y) ? (x) : (y)) 37 | #define MAX(x, y) ((x) > (y) ? (x) : (y)) 38 | 39 | void print_hex_dump(const char *prefix, const void *buf, size_t len); 40 | 41 | #ifndef container_of 42 | #define container_of(ptr, type, member) ({ \ 43 | const typeof(((type *)0)->member)*__mptr = (ptr); \ 44 | (type *)((char *)__mptr - offsetof(type, member)); \ 45 | }) 46 | #endif 47 | 48 | #define __packed __attribute__((packed)) 49 | 50 | #define BITS_TO_BYTES(n) (((n) + 7) / 8) 51 | #endif 52 | -------------------------------------------------------------------------------- /router/watch.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Bjorn Andersson 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "list.h" 48 | #include "mbuf.h" 49 | #include "util.h" 50 | #include "watch.h" 51 | 52 | #define FLOW_WATERMARK 10 53 | 54 | /** 55 | * struct watch_flow - flow control context 56 | * @packets: number of outstanding packets 57 | */ 58 | struct watch_flow { 59 | int packets; 60 | }; 61 | 62 | struct watch { 63 | int fd; 64 | int (*cb)(int, void*); 65 | void *data; 66 | struct list_head *queue; 67 | 68 | struct iocb iocb; 69 | struct mbuf *pending_aio; 70 | 71 | bool is_write; 72 | 73 | struct watch_flow *flow; 74 | 75 | int (*aio_complete)(struct mbuf *, void*); 76 | 77 | struct list_head node; 78 | }; 79 | 80 | struct timer { 81 | void (*cb)(void *); 82 | void *data; 83 | unsigned int interval; 84 | bool repeat; 85 | 86 | struct timeval tick; 87 | 88 | struct list_head node; 89 | }; 90 | 91 | static struct list_head timers = LIST_INIT(timers); 92 | 93 | static struct list_head read_watches = LIST_INIT(read_watches); 94 | static struct list_head aio_watches = LIST_INIT(aio_watches); 95 | static struct list_head quit_watches = LIST_INIT(quit_watches); 96 | static bool do_watch_quit; 97 | 98 | typedef unsigned long aio_context_t; 99 | 100 | static long io_destroy(aio_context_t ctx) 101 | { 102 | return syscall(__NR_io_destroy, ctx); 103 | } 104 | 105 | static long io_getevents(__attribute__((unused)) aio_context_t ctx, 106 | __attribute__((unused)) long min_nr, 107 | __attribute__((unused)) long nr, 108 | __attribute__((unused)) struct io_event *events, 109 | __attribute__((unused)) struct timespec *tmo) 110 | { 111 | #ifdef __NR_io_getevents 112 | return syscall(__NR_io_getevents, ctx, min_nr, nr, events, tmo); 113 | #else 114 | return -ENOSYS; 115 | #endif 116 | } 117 | 118 | static long io_setup(unsigned nr_reqs, aio_context_t *ctx) 119 | { 120 | return syscall(__NR_io_setup, nr_reqs, ctx); 121 | } 122 | 123 | static long io_submit(aio_context_t ctx, long n, struct iocb **paiocb) 124 | { 125 | return syscall(__NR_io_submit, ctx, n, paiocb); 126 | } 127 | 128 | struct watch_flow *watch_flow_new(void) 129 | { 130 | return calloc(1, sizeof(struct watch_flow)); 131 | } 132 | 133 | void watch_flow_inc(struct watch_flow *flow) 134 | { 135 | if (!flow) 136 | return; 137 | 138 | flow->packets++; 139 | } 140 | 141 | static void watch_flow_dec(struct watch_flow *flow) 142 | { 143 | if (!flow) 144 | return; 145 | 146 | if (!flow->packets) 147 | fprintf(stderr, "unbalanced flow control\n"); 148 | else 149 | flow->packets--; 150 | } 151 | 152 | static bool watch_flow_blocked(struct watch_flow *flow) 153 | { 154 | return flow && flow->packets > FLOW_WATERMARK; 155 | } 156 | 157 | int watch_add_readfd(int fd, int (*cb)(int, void*), void *data, 158 | struct watch_flow *flow) 159 | { 160 | struct watch *w; 161 | 162 | w = calloc(1, sizeof(struct watch)); 163 | if (!w) 164 | err(1, "calloc"); 165 | 166 | w->fd = fd; 167 | w->cb = cb; 168 | w->data = data; 169 | w->flow = flow; 170 | 171 | list_add(&read_watches, &w->node); 172 | 173 | return 0; 174 | } 175 | 176 | int watch_add_readq(int fd, struct list_head *queue, 177 | int (*cb)(struct mbuf *mbuf, void *data), void *data) 178 | { 179 | struct watch *w; 180 | 181 | w = calloc(1, sizeof(*w)); 182 | if (!w) 183 | err(1, "calloc"); 184 | 185 | w->fd = fd; 186 | w->aio_complete = cb; 187 | w->data = data; 188 | w->queue = queue; 189 | 190 | w->is_write = false; 191 | 192 | list_add(&aio_watches, &w->node); 193 | 194 | return 0; 195 | } 196 | 197 | static int watch_free_write_aio(struct mbuf *mbuf, void *data) 198 | { 199 | watch_flow_dec(mbuf->flow); 200 | free(mbuf); 201 | 202 | return 0; 203 | } 204 | 205 | int watch_add_writeq(int fd, struct list_head *queue) 206 | { 207 | struct watch *w; 208 | 209 | w = calloc(1, sizeof(*w)); 210 | if (!w) 211 | err(1, "calloc"); 212 | 213 | w->fd = fd; 214 | w->queue = queue; 215 | w->data = w; 216 | 217 | w->aio_complete = watch_free_write_aio; 218 | 219 | w->is_write = true; 220 | 221 | list_add(&aio_watches, &w->node); 222 | 223 | return 0; 224 | } 225 | 226 | void watch_remove_fd(int fd) 227 | { 228 | struct list_head *item; 229 | struct list_head *next; 230 | struct watch *w; 231 | 232 | list_for_each_safe(item, next, &read_watches) { 233 | w = container_of(item, struct watch, node); 234 | if (w->fd == fd) { 235 | list_del(&w->node); 236 | free(w); 237 | } 238 | } 239 | 240 | list_for_each_safe(item, next, &aio_watches) { 241 | w = container_of(item, struct watch, node); 242 | if (w->fd == fd) { 243 | list_del(&w->node); 244 | free(w); 245 | } 246 | } 247 | } 248 | 249 | void watch_remove_writeq(int fd) 250 | { 251 | struct list_head *item; 252 | struct list_head *next; 253 | struct watch *w; 254 | 255 | list_for_each_safe(item, next, &aio_watches) { 256 | w = container_of(item, struct watch, node); 257 | if (w->fd == fd) { 258 | list_del(&w->node); 259 | free(w); 260 | } 261 | } 262 | } 263 | 264 | int watch_add_quit(int (*cb)(int, void*), void *data) 265 | { 266 | struct watch *w; 267 | 268 | w = calloc(1, sizeof(struct watch)); 269 | if (!w) 270 | err(1, "calloc"); 271 | 272 | w->cb = cb; 273 | w->data = data; 274 | 275 | list_add(&quit_watches, &w->node); 276 | 277 | return 0; 278 | } 279 | 280 | static void watch_set_timer(struct timer *timer) 281 | { 282 | struct timeval now; 283 | struct timeval tv; 284 | int ret; 285 | 286 | ret = gettimeofday(&now, NULL); 287 | if (ret < 0) 288 | err(1, "failed to gettimeofday"); 289 | 290 | tv.tv_sec = timer->interval / 1000; 291 | tv.tv_usec = (timer->interval % 1000) * 1000; 292 | 293 | timeradd(&now, &tv, &timer->tick); 294 | } 295 | 296 | int watch_add_timer(void (*cb)(void *), void *data, unsigned int interval, 297 | bool repeat) 298 | { 299 | struct timer *w; 300 | 301 | w = calloc(1, sizeof(struct timer)); 302 | if (!w) 303 | err(1, "calloc"); 304 | 305 | w->cb = cb; 306 | w->data = data; 307 | w->interval = interval; 308 | w->repeat = repeat; 309 | 310 | list_add(&timers, &w->node); 311 | 312 | watch_set_timer(w); 313 | 314 | return 0; 315 | } 316 | 317 | static void watch_free_timer(struct timer *timer) 318 | { 319 | list_del(&timer->node); 320 | free(timer); 321 | } 322 | 323 | static struct timer *watch_get_next_timer() 324 | { 325 | struct timeval tv; 326 | struct timer *selected; 327 | struct timer *timer; 328 | 329 | if (list_empty(&timers)) 330 | return NULL; 331 | 332 | selected = container_of(timers.next, struct timer, node); 333 | tv = selected->tick; 334 | 335 | list_for_each_entry(timer, &timers, node) { 336 | if (timercmp(&timer->tick, &tv, <)) { 337 | selected = timer; 338 | tv = timer->tick; 339 | } 340 | } 341 | 342 | return selected; 343 | } 344 | 345 | void watch_quit(void) 346 | { 347 | do_watch_quit = true; 348 | } 349 | 350 | static void watch_submit_aio(aio_context_t ioctx, int evfd, struct watch *w) 351 | { 352 | struct iocb *iocb = &w->iocb; 353 | struct mbuf *mbuf; 354 | int ret; 355 | 356 | assert(!w->pending_aio); 357 | 358 | if (list_empty(w->queue)) 359 | return; 360 | 361 | mbuf = list_entry_first(w->queue, struct mbuf, node); 362 | 363 | memset(iocb, 0, sizeof(*iocb)); 364 | iocb->aio_fildes = w->fd; 365 | iocb->aio_lio_opcode = w->is_write ? IOCB_CMD_PWRITE : IOCB_CMD_PREAD; 366 | iocb->aio_buf = (uint64_t)mbuf->data; 367 | iocb->aio_nbytes = mbuf->size; 368 | iocb->aio_offset = 0; 369 | iocb->aio_flags = IOCB_FLAG_RESFD; 370 | iocb->aio_resfd = evfd; 371 | 372 | ret = io_submit(ioctx, 1, &iocb); 373 | if (ret != 1) 374 | fprintf(stderr, "io_submit failed: %d (%d)\n", ret, errno); 375 | 376 | if (ret == 1) { 377 | list_del(&mbuf->node); 378 | w->pending_aio = mbuf; 379 | } 380 | } 381 | 382 | static void watch_handle_eventfd(int evfd, aio_context_t ioctx) 383 | { 384 | struct io_event ev[32]; 385 | struct iocb *iocb; 386 | struct watch *next; 387 | struct watch *w; 388 | uint64_t evcnt; 389 | ssize_t n; 390 | int count; 391 | int i; 392 | 393 | n = read(evfd, &evcnt, sizeof(evcnt)); 394 | if (n < 0) { 395 | warn("failed to read eventfd counter"); 396 | return; 397 | } 398 | 399 | count = io_getevents(ioctx, 1, 32, ev, NULL); 400 | list_for_each_entry_safe(w, next, &aio_watches, node) { 401 | for (i = 0; i < count; i++) { 402 | iocb = (struct iocb *)ev[i].obj; 403 | if (iocb->aio_fildes == w->fd) { 404 | assert(w->pending_aio); 405 | 406 | if (ev[i].res == -EAGAIN) 407 | continue; 408 | 409 | if (!w->is_write && ev[i].res >= 0) 410 | w->pending_aio->offset = ev[i].res; 411 | 412 | w->aio_complete(w->pending_aio, w->data); 413 | w->pending_aio = NULL; 414 | 415 | // watch_submit_aio(ioctx, evfd, w); 416 | } 417 | } 418 | } 419 | } 420 | 421 | void watch_run(void) 422 | { 423 | struct timeval *timeout; 424 | struct timer *timer; 425 | struct timeval now; 426 | struct timeval tv; 427 | aio_context_t ioctx = 0; 428 | struct watch *next; 429 | struct watch *w; 430 | fd_set rfds; 431 | int evfd; 432 | int nfds; 433 | int ret; 434 | 435 | evfd = eventfd(0, 0); 436 | if (evfd < 0) 437 | err(1, "failed to create eventfd"); 438 | 439 | ret = io_setup(32, &ioctx); 440 | if (ret < 0) 441 | err(1, "failed to initialize aio context"); 442 | 443 | while (!do_watch_quit) { 444 | FD_ZERO(&rfds); 445 | FD_SET(evfd, &rfds); 446 | 447 | nfds = evfd + 1; 448 | 449 | list_for_each_entry(w, &read_watches, node) { 450 | /* Skip read watches with flows that are blocked */ 451 | if (watch_flow_blocked(w->flow)) 452 | continue; 453 | 454 | FD_SET(w->fd, &rfds); 455 | 456 | nfds = MAX(w->fd + 1, nfds); 457 | } 458 | 459 | list_for_each_entry(w, &aio_watches, node) { 460 | /* Submit AIO if none is pending */ 461 | if (!list_empty(w->queue) && !w->pending_aio) 462 | watch_submit_aio(ioctx, evfd, w); 463 | } 464 | 465 | timer = watch_get_next_timer(); 466 | if (timer) { 467 | gettimeofday(&now, NULL); 468 | timersub(&timer->tick, &now, &tv); 469 | 470 | if (tv.tv_sec < 0) 471 | tv.tv_sec = tv.tv_usec = 0; 472 | 473 | timeout = &tv; 474 | } else { 475 | timeout = NULL; 476 | } 477 | 478 | ret = select(nfds, &rfds, NULL, NULL, timeout); 479 | if (ret < 0) { 480 | warn("failed to select"); 481 | break; 482 | } 483 | 484 | if (ret == 0 && timer) { 485 | timer->cb(timer->data); 486 | 487 | if (timer->repeat) 488 | watch_set_timer(timer); 489 | else 490 | watch_free_timer(timer); 491 | } 492 | 493 | if (FD_ISSET(evfd, &rfds)) 494 | watch_handle_eventfd(evfd, ioctx); 495 | 496 | list_for_each_entry_safe(w, next, &read_watches, node) { 497 | if (FD_ISSET(w->fd, &rfds)) { 498 | ret = w->cb(w->fd, w->data); 499 | if (ret < 0) 500 | list_del(&w->node); 501 | } 502 | } 503 | } 504 | 505 | list_for_each_entry(w, &quit_watches, node) 506 | w->cb(-1, w->data); 507 | 508 | io_destroy(ioctx); 509 | } 510 | -------------------------------------------------------------------------------- /router/watch.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Bjorn Andersson 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | #ifndef __WATCH_H__ 32 | #define __WATCH_H__ 33 | 34 | #include 35 | #include "list.h" 36 | 37 | struct mbuf; 38 | struct watch_flow; 39 | 40 | int watch_add_readfd(int fd, int (*cb)(int, void*), void *data, 41 | struct watch_flow *flow); 42 | int watch_add_readq(int fd, struct list_head *queue, 43 | int (*cb)(struct mbuf *mbuf, void *data), void *data); 44 | int watch_add_writeq(int fd, struct list_head *queue); 45 | void watch_remove_fd(int fd); 46 | void watch_remove_writeq(int fd); 47 | int watch_add_quit(int (*cb)(int, void*), void *data); 48 | int watch_add_timer(void (*cb)(void *), void *data, 49 | unsigned int interval, bool repeat); 50 | void watch_quit(void); 51 | void watch_run(void); 52 | 53 | 54 | struct watch_flow; 55 | 56 | struct watch_flow *watch_flow_new(void); 57 | void watch_flow_inc(struct watch_flow *flow); 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /tools/send_data.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018, Linaro Ltd. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * 3. Neither the name of the copyright holder nor the names of its contributors 16 | * may be used to endorse or promote products derived from this software without 17 | * specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #define DIAG_CMD_RSP_BAD_COMMAND 0x13 48 | #define DIAG_CMD_RSP_BAD_PARAMS 0x14 49 | #define DIAG_CMD_RSP_BAD_LENGTH 0x15 50 | 51 | int main(int argc, char **argv) 52 | { 53 | struct sockaddr_un addr; 54 | unsigned char *msg; 55 | struct timeval tv = {5, 0}; 56 | fd_set rfds; 57 | ssize_t n; 58 | char buf[8192]; 59 | int ret; 60 | int fd; 61 | int i; 62 | 63 | msg = calloc(argc - 1, sizeof(*msg)); 64 | 65 | for (i = 1; i < argc; i++) 66 | msg[i - 1] = atoi(argv[i]); 67 | 68 | fd = socket(AF_UNIX, SOCK_SEQPACKET, 0); 69 | if (fd < 0) 70 | err(1, "failed to create unix socket"); 71 | 72 | memset(&addr, 0, sizeof(addr)); 73 | addr.sun_family = AF_UNIX; 74 | strncpy(addr.sun_path, "\0diag", sizeof(addr.sun_path)-1); 75 | 76 | ret = connect(fd, (struct sockaddr*)&addr, sizeof(addr)); 77 | if (ret < 0) 78 | err(1, "failed to connect to diag"); 79 | 80 | n = write(fd, msg, argc - 1); 81 | if (n < 0) 82 | err(1, "failed to send request"); 83 | 84 | for (;;) { 85 | FD_ZERO(&rfds); 86 | FD_SET(fd, &rfds); 87 | 88 | ret = select(fd + 1, &rfds, NULL, NULL, &tv); 89 | if (!ret) { 90 | fprintf(stderr, "timeout waiting for response\n"); 91 | exit(1); 92 | } 93 | 94 | if (!FD_ISSET(fd, &rfds)) 95 | continue; 96 | 97 | n = read(fd, buf, sizeof(buf)); 98 | if (!n) { 99 | fprintf(stderr, "disconnected\n"); 100 | exit(1); 101 | } else if (n < 0) { 102 | fprintf(stderr, "failed to read response: %s\n", 103 | strerror(errno)); 104 | exit(1); 105 | } 106 | 107 | if (buf[0] == DIAG_CMD_RSP_BAD_COMMAND) { 108 | printf("Diag response: Bad command\n"); 109 | break; 110 | } 111 | 112 | for (i = 0; i < n; i++) { 113 | printf("%s%d", i == 0 ? "" : " ", buf[i]); 114 | if (i % 16 == 15 || i == n - 1) 115 | printf("\n"); 116 | } 117 | 118 | if (buf[0] == msg[0]) 119 | break; 120 | } 121 | 122 | return 0; 123 | } 124 | 125 | --------------------------------------------------------------------------------