Harald Welte | ccea8dd | 2016-12-24 10:27:55 +0100 | [diff] [blame] | 1 | /* |
| 2 | * (C) 2013-2016 by Harald Welte <laforge@gnumonks.org> |
| 3 | * |
| 4 | * This program is free software; you can redistribute it and/or modify |
| 5 | * it under the terms of the GNU General Public License as published by |
| 6 | * the Free Software Foundation; either version 2 of the License, or |
| 7 | * (at your option) any later version. |
| 8 | * |
| 9 | * This program is distributed in the hope that it will be useful, |
| 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 12 | * GNU General Public License for more details. |
| 13 | * |
| 14 | * You should have received a copy of the GNU General Public License along |
| 15 | * with this program; if not, write to the Free Software Foundation, Inc., |
| 16 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. |
| 17 | */ |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 18 | |
| 19 | #include <stdlib.h> |
| 20 | #include <stdio.h> |
| 21 | #include <string.h> |
| 22 | |
| 23 | #include <osmocom/core/msgb.h> |
Harald Welte | 6fc7f68 | 2016-12-24 10:01:28 +0100 | [diff] [blame] | 24 | #include <osmocom/core/bit16gen.h> |
| 25 | #include <osmocom/core/bit32gen.h> |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 26 | |
Harald Welte | 84ec50f | 2016-12-24 10:16:00 +0100 | [diff] [blame] | 27 | #include "protocol/protocol.h" |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 28 | #include "diag_msg.h" |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 29 | #include "diag_cmd.h" |
Harald Welte | 84ec50f | 2016-12-24 10:16:00 +0100 | [diff] [blame] | 30 | #include "protocol/diagcmd.h" |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 31 | |
Harald Welte | 6fc7f68 | 2016-12-24 10:01:28 +0100 | [diff] [blame] | 32 | struct diag_set_rt_mask_req { |
| 33 | uint8_t cmd_code; |
| 34 | uint8_t sub_cmd; |
| 35 | uint16_t ssid_start; |
| 36 | uint16_t ssid_end; |
| 37 | uint16_t _pad; |
| 38 | uint32_t runtime_mask[1]; |
| 39 | }; |
| 40 | |
| 41 | #define MSG_EXT_SUBCMD_SET_RT_MASK 4 |
| 42 | |
| 43 | struct msgb *gen_msg_config_set_rt_mask(uint16_t ssid, uint32_t runtime_mask) |
| 44 | { |
| 45 | struct msgb *msg = msgb_alloc(DIAG_MAX_REQ_SIZE, "Diag Msg Config"); |
| 46 | struct diag_set_rt_mask_req *dsrmr; |
| 47 | |
| 48 | msg->l2h = msgb_put(msg, sizeof(*dsrmr)); |
Harald Welte | 4ae0936 | 2017-01-07 15:54:41 +0100 | [diff] [blame] | 49 | dsrmr = (struct diag_set_rt_mask_req *) msgb_l2(msg); |
Harald Welte | 6fc7f68 | 2016-12-24 10:01:28 +0100 | [diff] [blame] | 50 | dsrmr->cmd_code = DIAG_EXT_MSG_CONFIG_F; |
| 51 | dsrmr->sub_cmd = MSG_EXT_SUBCMD_SET_RT_MASK; |
| 52 | osmo_store16le(ssid, &dsrmr->ssid_start); |
| 53 | osmo_store16le(ssid, &dsrmr->ssid_end); |
| 54 | osmo_store32le(runtime_mask, &dsrmr->runtime_mask[0]); |
| 55 | |
| 56 | return msg; |
| 57 | } |
| 58 | |
| 59 | int diag_msg_config_set_rt_mask(struct diag_instance *di, uint16_t ssid, uint32_t runtime_mask) |
| 60 | { |
| 61 | struct msgb *msg = gen_msg_config_set_rt_mask(ssid, runtime_mask); |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 62 | struct msgb *rx; |
Harald Welte | f180199 | 2017-01-01 11:47:31 +0100 | [diff] [blame] | 63 | struct diag_set_rt_mask_req *res; |
| 64 | int rc = 0; |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 65 | |
| 66 | rx = diag_transceive_msg(di, msg); |
Harald Welte | f180199 | 2017-01-01 11:47:31 +0100 | [diff] [blame] | 67 | res = (struct diag_set_rt_mask_req *) (msgb_l2(msg)+1); |
Harald Welte | 651d4d8 | 2017-01-02 00:53:11 +0100 | [diff] [blame] | 68 | if ((rx->l2h[0] != DIAG_EXT_MSG_CONFIG_F) || res->cmd_code != MSG_EXT_SUBCMD_SET_RT_MASK || |
Harald Welte | f180199 | 2017-01-01 11:47:31 +0100 | [diff] [blame] | 69 | res->sub_cmd != MSG_EXT_SUBCMD_SET_RT_MASK || |
| 70 | osmo_load16le(&res->ssid_start) != ssid || |
| 71 | osmo_load16le(&res->ssid_end) != ssid || |
| 72 | osmo_load32le(&res->runtime_mask) != runtime_mask) { |
| 73 | fprintf(stderr, "Error setting RT mask\n"); |
| 74 | rc = -1; |
| 75 | } |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 76 | msgb_free(rx); |
Harald Welte | 6fc7f68 | 2016-12-24 10:01:28 +0100 | [diff] [blame] | 77 | |
Harald Welte | f180199 | 2017-01-01 11:47:31 +0100 | [diff] [blame] | 78 | return rc; |
Harald Welte | 6fc7f68 | 2016-12-24 10:01:28 +0100 | [diff] [blame] | 79 | } |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 80 | |
| 81 | /* handler for EXT MSG */ |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 82 | static void diag_rx_ext_msg_f(struct diag_instance *di, struct msgb *msgb) |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 83 | { |
Harald Welte | 70ff72d | 2017-01-21 11:20:07 +0100 | [diff] [blame^] | 84 | uint8_t *data = msgb_data(msgb); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 85 | const size_t len = msgb_length(msgb); |
Harald Welte | 70ff72d | 2017-01-21 11:20:07 +0100 | [diff] [blame^] | 86 | struct ext_log_msg *msg; |
| 87 | char *file = NULL, *fmt; |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 88 | unsigned int num_args; |
| 89 | |
| 90 | if (len < sizeof(struct ext_log_msg)) { |
| 91 | printf("too short ext_log_msg.\n"); |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 92 | return; |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 93 | } |
| 94 | |
| 95 | msg = (struct ext_log_msg *) data; |
Harald Welte | 89c159c | 2017-01-01 16:42:12 +0100 | [diff] [blame] | 96 | num_args = msg->hdr.num_args; |
Harald Welte | 70ff72d | 2017-01-21 11:20:07 +0100 | [diff] [blame^] | 97 | fmt = (char *) msg->params + num_args*sizeof(msg->params[0]); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 98 | file = fmt + strlen(fmt) + 1; |
| 99 | |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 100 | printf("MSG(%u|%u|%s:%u): ", osmo_load16le(&msg->subsys_id), |
Harald Welte | 89c159c | 2017-01-01 16:42:12 +0100 | [diff] [blame] | 101 | diag_ts_to_epoch(osmo_load64le(&msg->hdr.timestamp)), |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 102 | file, osmo_load16le(&msg->line_nr)); |
Harald Welte | b8a7098 | 2017-01-08 13:45:54 +0100 | [diff] [blame] | 103 | |
| 104 | /* replace all '%s' with '%p', as %s obviously doesn't work */ |
| 105 | for (char *cur = fmt; cur && (cur < fmt + strlen(fmt)); cur = strstr(fmt, "%s")) { |
| 106 | cur[1] = 'p'; |
| 107 | } |
| 108 | |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 109 | switch (num_args) { |
| 110 | case 0: |
| 111 | fputs(fmt, stdout); |
| 112 | break; |
| 113 | case 1: |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 114 | printf(fmt, osmo_load32le(&msg->params[0])); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 115 | break; |
| 116 | case 2: |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 117 | printf(fmt, osmo_load32le(&msg->params[0]), |
| 118 | osmo_load32le(&msg->params[1])); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 119 | break; |
| 120 | case 3: |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 121 | printf(fmt, osmo_load32le(&msg->params[0]), |
| 122 | osmo_load32le(&msg->params[1]), |
| 123 | osmo_load32le(&msg->params[2])); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 124 | break; |
| 125 | case 4: |
Harald Welte | 7af93aa | 2017-01-01 12:00:38 +0100 | [diff] [blame] | 126 | printf(fmt, osmo_load32le(&msg->params[0]), |
| 127 | osmo_load32le(&msg->params[1]), |
| 128 | osmo_load32le(&msg->params[2]), |
| 129 | osmo_load32le(&msg->params[3])); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 130 | break; |
| 131 | } |
| 132 | fputc('\n', stdout); |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 133 | } |
| 134 | |
Harald Welte | 89c159c | 2017-01-01 16:42:12 +0100 | [diff] [blame] | 135 | static void diag_rx_ext_msg_terse_f(struct diag_instance *di, struct msgb *msgb) |
| 136 | { |
| 137 | const uint8_t *data = msgb_data(msgb); |
| 138 | const size_t len = msgb_length(msgb); |
| 139 | const struct qsr_ext_msg_terse *msg; |
| 140 | unsigned int num_args; |
| 141 | |
| 142 | if (len < sizeof(struct qsr_ext_msg_terse)) { |
| 143 | printf("too short ext_log_msg.\n"); |
| 144 | return; |
| 145 | } |
| 146 | |
| 147 | msg = (struct qsr_ext_msg_terse *) data; |
| 148 | num_args = msg->hdr.num_args; |
| 149 | |
| 150 | printf("MSG_QS(%u|%u|%08x:%u): ", osmo_load16le(&msg->subsys_id), |
| 151 | diag_ts_to_epoch(osmo_load64le(&msg->hdr.timestamp)), |
| 152 | osmo_load32le(&msg->hash), osmo_load16le(&msg->line_nr)); |
| 153 | switch (num_args) { |
| 154 | case 0: |
| 155 | fputs("", stdout); |
| 156 | break; |
| 157 | case 1: |
| 158 | printf("%08x", osmo_load32le(&msg->params[0])); |
| 159 | break; |
| 160 | case 2: |
| 161 | printf("%08x %08x", osmo_load32le(&msg->params[0]), |
| 162 | osmo_load32le(&msg->params[1])); |
| 163 | break; |
| 164 | case 3: |
| 165 | printf("%08x %08x %08x", osmo_load32le(&msg->params[0]), |
| 166 | osmo_load32le(&msg->params[1]), |
| 167 | osmo_load32le(&msg->params[2])); |
| 168 | break; |
| 169 | case 4: |
| 170 | printf("%08x %08x %08x %08x", osmo_load32le(&msg->params[0]), |
| 171 | osmo_load32le(&msg->params[1]), |
| 172 | osmo_load32le(&msg->params[2]), |
| 173 | osmo_load32le(&msg->params[3])); |
| 174 | break; |
| 175 | } |
| 176 | fputc('\n', stdout); |
| 177 | } |
| 178 | |
| 179 | |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 180 | struct diag_cmd_dispatch_tbl cmd_tbl[] = { |
| 181 | { DIAG_EXT_MSG_F, diag_rx_ext_msg_f }, |
Harald Welte | 89c159c | 2017-01-01 16:42:12 +0100 | [diff] [blame] | 182 | { DIAG_QSR_EXT_MSG_TERSE_F, diag_rx_ext_msg_terse_f }, |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 183 | }; |
Harald Welte | 0e50916 | 2016-12-24 01:32:10 +0100 | [diff] [blame] | 184 | |
Harald Welte | 022c45a | 2017-01-01 16:28:57 +0100 | [diff] [blame] | 185 | static __attribute__((constructor)) void on_dso_load_msg(void) |
| 186 | { |
| 187 | diag_cmd_reg_dispatch(cmd_tbl, ARRAY_SIZE(cmd_tbl)); |
| 188 | } |