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