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         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 }