• source navigation  • diff markup  • identifier search  • freetext search  • 

Sources/umbim/mbim-msg.c

  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         int l = strlen(in);
 57         int s = mbim_add_payload(l * 2);
 58         uint8_t *p = &payload_buffer[s];
 59         int i;
 60 
 61         if (!s)
 62                 return -1;
 63 
 64         str->offset = htole32(s);
 65         str->length = htole32(l * 2);
 66         for (i = 0; i < l; i++)
 67                 p[i * 2] = in[i];
 68 
 69         return 0;
 70 }
 71 
 72 
 73 char *
 74 mbim_get_string(struct mbim_string *str, char *in)
 75 {
 76         char *p = &in[le32toh(str->offset)];
 77         unsigned int i;
 78 
 79         if (!le32toh(str->offset))
 80                 return NULL;
 81 
 82         if (le32toh(str->length)) {
 83                 for (i = 0; i < le32toh(str->length) / 2; i++)
 84                         p[i] = p[i * 2];
 85                 p[i] = '\0';
 86                 str->length = 0;
 87         }
 88 
 89         return p;
 90 }
 91 
 92 void
 93 mbim_get_ipv4(void *buffer, char *out, uint32_t offset)
 94 {
 95         uint8_t *b = buffer + offset;
 96 
 97         snprintf(out, 16, "%d.%d.%d.%d", b[0], b[1], b[2], b[3]);
 98 }
 99 
100 void
101 mbim_get_ipv6(void *buffer, char *out, uint32_t offset)
102 {
103         uint8_t *b = buffer + offset;
104 
105         snprintf(out, 40, "%x:%x:%x:%x:%x:%x:%x:%x", b[0] << 8 | b[1],
106                  b[2] << 8 | b[3], b[4] << 8 | b[5], b[6] << 8 | b[7],
107                  b[8] << 8 | b[9], b[10] << 8 | b[11], b[12] << 8 | b[13],
108                  b[14] << 8 | b[15]);
109 }
110 
111 uint32_t
112 mbim_get_int(void *buffer, uint32_t offset)
113 {
114         uint32_t *i = buffer + offset;
115 
116         return le32toh(*i);
117 }
118 
119 const char*
120 mbim_enum_string(struct mbim_enum *e, uint32_t key)
121 {
122         while (e->skey) {
123                 if (key == e->key)
124                         return e->val;
125                 e++;
126         }
127         return NULL;
128 }
129 
130 void
131 mbim_setup_header(struct mbim_message_header *hdr, MbimMessageType type, int length)
132 {
133         if (length < 16)
134                 length = 16;
135 
136         hdr->transaction_id = htole32(transaction_id++);
137         hdr->type = htole32(type);
138         hdr->length = htole32(length);
139 }
140 
141 uint8_t*
142 mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int len)
143 {
144         struct command_message *cmd = (struct command_message *) mbim_buffer;
145 
146         if (!mbim_buffer)
147                 return NULL;
148         memset(mbim_buffer, 0, mbim_bufsize);
149 
150         cmd->fragment_header.total = htole32(1);
151         cmd->fragment_header.current = htole32(0);
152         memcpy(cmd->service_id, uuid, 16);
153         cmd->command_id = htole32(command_id);
154         cmd->command_type = htole32(type);
155         cmd->buffer_length = htole32(len);
156 
157         payload_offset = len;
158         payload_free = mbim_bufsize - (sizeof(*cmd) + len);
159         payload_len = 0;
160         payload_buffer = cmd->buffer;
161 
162         return cmd->buffer;
163 }
164 
165 int
166 mbim_send_command_msg(void)
167 {
168         struct command_message *cmd = (struct command_message *) mbim_buffer;
169 
170         if (!mbim_buffer)
171                 return 0;
172         if (payload_len & 0x3) {
173                 payload_len &= ~0x3;
174                 payload_len += 4;
175         }
176 
177         cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len);
178         mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length));
179 
180         return mbim_send();
181 }
182 
183 int
184 mbim_send_open_msg(void)
185 {
186         struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer;
187 
188         mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg));
189         msg->max_control_transfer = htole32(mbim_bufsize);
190 
191         return mbim_send();
192 }
193 
194 int
195 mbim_send_close_msg(void)
196 {
197         struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
198 
199         if (no_close || !mbim_buffer) {
200                 mbim_end();
201                 return 0;
202         }
203         mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));
204 
205         return mbim_send();
206 }
207 

This page was automatically generated by LXR 0.3.1.  •  OpenWrt