1 /* 2 * uqmi -- tiny QMI support implementation 3 * 4 * Copyright (C) 2023 Alexander Couzens <lynxis@fe80.eu> 5 * 6 * This library is free software; you can redistribute it and/or 7 * modify it under the terms of the GNU General Public 8 * License as published by the Free Software Foundation; either 9 * version 2 of the License, or (at your option) any later version. 10 * 11 * This library is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 * General Public License for more details. 15 */ 16 17 #include <arpa/inet.h> 18 #include <netinet/in.h> 19 #include <stdbool.h> 20 #include <string.h> 21 #include <sys/socket.h> 22 23 #include <talloc.h> 24 #include <unistd.h> 25 #include "gsmtapv3.h" 26 27 #include "gsmtap_util.h" 28 29 struct t16l16v { 30 uint16_t type; 31 uint16_t length; 32 uint8_t value[0]; 33 } __attribute__((packed)); 34 35 #define GSMTAPV3_BB_DIAG_QC_QMI 1 36 37 struct gsmtap_instance { 38 int fd; 39 bool valid; 40 }; 41 42 static struct gsmtap_instance gsmtap_inst; 43 44 void gsmtap_disable(void) 45 { 46 if (!gsmtap_inst.valid) 47 return; 48 49 if (gsmtap_inst.fd >= 0) { 50 close(gsmtap_inst.fd); 51 gsmtap_inst.fd = -1; 52 } 53 54 gsmtap_inst.valid = false; 55 } 56 57 /* add support for IPv6 */ 58 int gsmtap_enable(const char *gsmtap_addr) 59 { 60 int ret, sfd; 61 struct sockaddr_in in = {}; 62 63 gsmtap_disable(); 64 sfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); 65 if (sfd < 0) 66 return -1; 67 68 in.sin_addr.s_addr = inet_addr(gsmtap_addr); 69 in.sin_port = htons(GSMTAPV3_UDP_PORT); 70 in.sin_family = AF_INET; 71 ret = connect(sfd, (struct sockaddr *) &in, sizeof(struct sockaddr_in)); 72 if (ret < 0) 73 return -1; 74 75 gsmtap_inst.fd = sfd; 76 gsmtap_inst.valid = true; 77 78 return 0; 79 } 80 81 static void tx_gsmtap(void *msg, size_t length) 82 { 83 if (!gsmtap_inst.valid) 84 return ; 85 86 write(gsmtap_inst.fd, msg, length); 87 } 88 89 void gsmtap_send(struct modem *modem, void *data, size_t length) 90 { 91 if (!gsmtap_inst.valid) 92 return; 93 94 /* WARNING. GSMTAPv3 is still under development and defines will change! */ 95 void *user_data; 96 void *msg = talloc_size(modem, length + sizeof(struct gsmtap_hdr_v3) + 32); 97 struct gsmtap_hdr_v3 *gsmtap = msg; 98 struct t16l16v *metadata = msg + sizeof(struct gsmtap_hdr_v3); 99 int meta_len = 0, rest; 100 size_t packet_len = 0; 101 102 gsmtap->version = GSMTAPV3_VERSION; 103 gsmtap->res = 0; 104 gsmtap->hdr_len = sizeof(struct gsmtap_hdr_v3) >> 2; 105 gsmtap->type = htons(GSMTAPV3_TYPE_BASEBAND_DIAG); 106 gsmtap->sub_type = htons(GSMTAPV3_BASEBAND_DIAG_QUALCOMM); 107 108 metadata->type = 0x1; 109 metadata->length = 0x1; 110 metadata->value[0] = 0x1; 111 112 meta_len = sizeof(*metadata) + metadata->length; 113 rest = meta_len % 4; 114 if (rest) 115 meta_len += 4 - rest; 116 117 gsmtap->hdr_len += (meta_len >> 2); 118 gsmtap->hdr_len = htons(gsmtap->hdr_len); 119 120 user_data = msg + sizeof(struct gsmtap_hdr_v3) + meta_len; 121 memcpy(user_data, data, length); 122 123 packet_len = length + sizeof(struct gsmtap_hdr_v3) + meta_len; 124 tx_gsmtap(msg, packet_len); 125 } 126
This page was automatically generated by LXR 0.3.1. • OpenWrt