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 | */ |
| 18 | |
Harald Welte | faea754 | 2016-12-24 01:21:03 +0100 | [diff] [blame] | 19 | #include <stdio.h> |
| 20 | #include <string.h> |
| 21 | #include <stdint.h> |
| 22 | #include <osmocom/core/msgb.h> |
| 23 | |
| 24 | #include "diag_log.h" |
Harald Welte | 84ec50f | 2016-12-24 10:16:00 +0100 | [diff] [blame] | 25 | #include "protocol/protocol.h" |
| 26 | #include "protocol/diagcmd.h" |
Harald Welte | faea754 | 2016-12-24 01:21:03 +0100 | [diff] [blame] | 27 | |
| 28 | /*********************************************************************** |
| 29 | * LOG Configuration / Protocol |
| 30 | ***********************************************************************/ |
| 31 | |
| 32 | enum log_config_op { |
| 33 | LOG_CONFIG_DISABLE_OP = 0, |
| 34 | LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1, |
| 35 | LOG_CONFIG_RETRIEVE_VALID_MASK_OP = 2, |
| 36 | LOG_CONFIG_SET_MASK_OP = 3, |
| 37 | LOG_CONFIG_GET_LOGMASK_OP = 4, |
| 38 | }; |
| 39 | |
| 40 | struct diag_log_config_req_hdr { |
| 41 | uint8_t msg_type; |
| 42 | uint8_t pad[3]; |
| 43 | uint32_t operation; |
| 44 | } __attribute((packed)); |
| 45 | |
| 46 | struct diag_log_config_set_mask { |
| 47 | struct diag_log_config_req_hdr hdr; |
| 48 | uint32_t equip_id; |
| 49 | uint32_t last_item; |
| 50 | uint8_t data[0]; |
| 51 | } __attribute((packed)); |
| 52 | |
| 53 | struct msgb *gen_log_config_set_mask(uint32_t equip_id, uint32_t last_item) |
| 54 | { |
| 55 | struct msgb *msg = msgb_alloc(DIAG_MAX_REQ_SIZE, "Diag Tx"); |
| 56 | struct diag_log_config_set_mask *dlcsm; |
| 57 | |
| 58 | msg->l2h = msgb_put(msg, sizeof(*dlcsm)); |
| 59 | dlcsm = (struct diag_log_config_set_mask *) msg->l2h; |
| 60 | dlcsm->hdr.msg_type = DIAG_LOG_CONFIG_F; |
| 61 | dlcsm->hdr.operation = LOG_CONFIG_SET_MASK_OP; |
| 62 | dlcsm->equip_id = equip_id; |
| 63 | dlcsm->last_item = last_item; |
| 64 | msg->l3h = msgb_put(msg, dlcsm->last_item/8); |
| 65 | |
| 66 | return msg; |
| 67 | } |
| 68 | |
| 69 | int log_config_set_mask_bit(struct msgb *msg, uint32_t bit_in) |
| 70 | { |
| 71 | struct diag_log_config_set_mask *dlcsm; |
| 72 | dlcsm = (struct diag_log_config_set_mask *) msg->l2h; |
| 73 | uint8_t *mask = msg->l3h; |
| 74 | unsigned int byte = bit_in / 8; |
| 75 | unsigned int bit = bit_in % 8; |
| 76 | |
| 77 | if (byte > dlcsm->last_item/8) |
| 78 | return -1; |
| 79 | |
| 80 | mask[byte] |= (1 << bit); |
| 81 | |
| 82 | return 0; |
| 83 | } |
| 84 | |
| 85 | |
| 86 | /*********************************************************************** |
| 87 | * LOG Dispatch |
| 88 | ***********************************************************************/ |
| 89 | |
| 90 | /* not particularly memory efficient, but welll, only 500kB on 64bit */ |
| 91 | static diag_log_handler *log_handlers[0xffff]; |
| 92 | |
| 93 | /* called by individual modules to register their own decoders */ |
| 94 | void diag_log_reg_dispatch(const struct diag_log_dispatch_tbl *tbl, unsigned int size) |
| 95 | { |
| 96 | unsigned int i; |
| 97 | for (i = 0; i < size; i++) { |
| 98 | printf("Registering dispatch for 0x%04x\n", tbl[i].code); |
| 99 | log_handlers[tbl[i].code] = tbl[i].handler; |
| 100 | } |
| 101 | } |
| 102 | |
| 103 | void diag_log_enable_all_supported_family(struct diag_instance *di, uint8_t family) |
| 104 | { |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 105 | struct msgb *msg, *rx; |
Harald Welte | faea754 | 2016-12-24 01:21:03 +0100 | [diff] [blame] | 106 | unsigned int i, size; |
| 107 | unsigned int family_base = (family & 0xf) << 12; |
| 108 | unsigned int max = 0; |
| 109 | |
| 110 | for (i = family_base; i < family_base + 0x1000; i++) { |
| 111 | if (log_handlers[i]) { |
| 112 | if (max < i) |
| 113 | max = i; |
| 114 | } |
| 115 | } |
| 116 | |
| 117 | if (!max) |
| 118 | return; |
| 119 | |
| 120 | size = max - family_base; |
| 121 | printf("family %u: allocating log mask of size %u\n", family, size); |
| 122 | msg = gen_log_config_set_mask(family, size); |
| 123 | for (i = family_base; i < family_base + 0x1000; i++) { |
| 124 | if (log_handlers[i]) |
| 125 | log_config_set_mask_bit(msg, i-family_base); |
| 126 | } |
| 127 | |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 128 | rx = diag_transceive_msg(di, msg); |
Harald Welte | e420463 | 2017-01-01 11:55:48 +0100 | [diff] [blame] | 129 | if (rx->l2h[0] != 0x53) |
| 130 | fprintf(stderr, "Error enabling logs for family %d\n", family); |
| 131 | /* FIXME: further validation of response */ |
Harald Welte | dc2cafc | 2017-01-01 11:15:35 +0100 | [diff] [blame] | 132 | msgb_free(rx); |
Harald Welte | faea754 | 2016-12-24 01:21:03 +0100 | [diff] [blame] | 133 | } |
| 134 | |
| 135 | void diag_log_enable_all_supported(struct diag_instance *di) |
| 136 | { |
| 137 | unsigned int i; |
| 138 | |
| 139 | for (i = 0; i < 0xF; i++) { |
| 140 | diag_log_enable_all_supported_family(di, i); |
| 141 | } |
| 142 | } |
| 143 | |
| 144 | void diag_log_handle(struct diag_instance *di, struct msgb *msg) |
| 145 | { |
| 146 | struct diag_log_hdr *dlh; |
| 147 | struct log_hdr *lh; |
| 148 | |
| 149 | dlh = (struct diag_log_hdr *) msg->data; |
| 150 | /* FIXME: verify length */ |
| 151 | msg->l3h = msgb_pull(msg, sizeof(*dlh)); |
| 152 | |
| 153 | lh = (struct log_hdr *) msg->l3h; |
| 154 | /* FIXME: verify length */ |
| 155 | msgb_pull(msg, sizeof(*lh)); |
| 156 | |
| 157 | printf("LOG(0x%04x|%u|%u): ", lh->code, |
| 158 | diag_ts_to_epoch(lh->ts), diag_ts_to_fn(lh->ts)); |
| 159 | |
| 160 | if (log_handlers[lh->code]) |
| 161 | log_handlers[lh->code](lh, msg); |
| 162 | else |
| 163 | printf("%s\n", osmo_hexdump(lh->data, lh->len)); |
| 164 | } |