1 /* 2 * umbim 3 * Copyright (C) 2014 John Crispin <blogic@openwrt.org> 4 * 5 * This program is free software; you can redistribute it and/or modify 6 * it under the terms of the GNU General Public License version 2 7 * as published by the Free Software Foundation 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 15 #include <sys/types.h> 16 #include <sys/stat.h> 17 18 #include <alloca.h> 19 #include <fcntl.h> 20 #include <stdio.h> 21 #include <stdlib.h> 22 #include <string.h> 23 #include <unistd.h> 24 25 #include <libubox/utils.h> 26 #include <libubox/uloop.h> 27 28 #include "mbim.h" 29 30 #include "data/mbim-service-basic-connect.h" 31 32 int transaction_id = 1; 33 uint8_t basic_connect[16] = { 0xa2, 0x89, 0xcc, 0x33, 0xbc, 0xbb, 0x8b, 0x4f, 34 0xb6, 0xb0, 0x13, 0x3e, 0xc2, 0xaa, 0xe6,0xdf }; 35 static int payload_offset, payload_free, payload_len; 36 static uint8_t *payload_buffer; 37 38 int 39 mbim_add_payload(uint8_t len) 40 { 41 uint32_t offset = payload_offset; 42 43 if (payload_free < len) 44 return 0; 45 46 payload_free -= len; 47 payload_offset += len; 48 payload_len += len; 49 50 return offset; 51 } 52 53 int 54 mbim_encode_string(struct mbim_string *str, char *in) 55 { 56 const int l = strlen(in); 57 const int utf16_len = l * 2; 58 const int pad_len = utf16_len % 4; 59 const int s = mbim_add_payload(utf16_len + pad_len); 60 uint8_t *p = &payload_buffer[s]; 61 int i; 62 63 if (!s) 64 return -1; 65 66 str->offset = htole32(s); 67 str->length = htole32(utf16_len); 68 69 for (i = 0; i < l; i++) 70 p[i * 2] = in[i]; 71 72 return 0; 73 } 74 75 char * 76 mbim_get_string(struct mbim_string *str, char *in) 77 { 78 char *p = &in[le32toh(str->offset)]; 79 unsigned int i; 80 81 if (!le32toh(str->offset)) 82 return NULL; 83 84 if (le32toh(str->length)) { 85 for (i = 0; i < le32toh(str->length) / 2; i++) 86 p[i] = p[i * 2]; 87 p[i] = '\0'; 88 str->length = 0; 89 } 90 91 return p; 92 } 93 94 void 95 mbim_get_ipv4(void *buffer, char *out, uint32_t offset) 96 { 97 uint8_t *b = buffer + offset; 98 99 snprintf(out, 16, "%d.%d.%d.%d", b[0], b[1], b[2], b[3]); 100 } 101 102 void 103 mbim_get_ipv6(void *buffer, char *out, uint32_t offset) 104 { 105 uint8_t *b = buffer + offset; 106 107 snprintf(out, 40, "%x:%x:%x:%x:%x:%x:%x:%x", b[0] << 8 | b[1], 108 b[2] << 8 | b[3], b[4] << 8 | b[5], b[6] << 8 | b[7], 109 b[8] << 8 | b[9], b[10] << 8 | b[11], b[12] << 8 | b[13], 110 b[14] << 8 | b[15]); 111 } 112 113 uint32_t 114 mbim_get_int(void *buffer, uint32_t offset) 115 { 116 uint32_t *i = buffer + offset; 117 118 return le32toh(*i); 119 } 120 121 const char* 122 mbim_enum_string(struct mbim_enum *e, uint32_t key) 123 { 124 while (e->skey) { 125 if (key == e->key) 126 return e->val; 127 e++; 128 } 129 return NULL; 130 } 131 132 void 133 mbim_setup_header(struct mbim_message_header *hdr, MbimMessageType type, int length) 134 { 135 if (length < 16) 136 length = 16; 137 138 hdr->transaction_id = htole32(transaction_id++); 139 hdr->type = htole32(type); 140 hdr->length = htole32(length); 141 } 142 143 uint8_t* 144 mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int len) 145 { 146 struct command_message *cmd = (struct command_message *) mbim_buffer; 147 148 if (!mbim_buffer) 149 return NULL; 150 memset(mbim_buffer, 0, mbim_bufsize); 151 152 cmd->fragment_header.total = htole32(1); 153 cmd->fragment_header.current = htole32(0); 154 memcpy(cmd->service_id, uuid, 16); 155 cmd->command_id = htole32(command_id); 156 cmd->command_type = htole32(type); 157 cmd->buffer_length = htole32(len); 158 159 payload_offset = len; 160 payload_free = mbim_bufsize - (sizeof(*cmd) + len); 161 payload_len = 0; 162 payload_buffer = cmd->buffer; 163 164 return cmd->buffer; 165 } 166 167 int 168 mbim_send_command_msg(void) 169 { 170 struct command_message *cmd = (struct command_message *) mbim_buffer; 171 172 if (!mbim_buffer) 173 return 0; 174 if (payload_len & 0x3) { 175 payload_len &= ~0x3; 176 payload_len += 4; 177 } 178 179 cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len); 180 mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length)); 181 182 return mbim_send(); 183 } 184 185 int 186 mbim_send_open_msg(void) 187 { 188 struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer; 189 190 mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg)); 191 msg->max_control_transfer = htole32(mbim_bufsize); 192 193 return mbim_send(); 194 } 195 196 int 197 mbim_send_close_msg(void) 198 { 199 struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer; 200 201 if (no_close || !mbim_buffer) { 202 mbim_end(); 203 return 0; 204 } 205 mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr)); 206 207 return mbim_send(); 208 } 209
This page was automatically generated by LXR 0.3.1. • OpenWrt