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         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         memset(mbim_buffer, 0, MBIM_BUFFER_SIZE);
 147
 148         cmd->fragment_header.total = htole32(1);
 149         cmd->fragment_header.current = htole32(0);
 150         memcpy(cmd->service_id, uuid, 16);
 151         cmd->command_id = htole32(command_id);
 152         cmd->command_type = htole32(type);
 153         cmd->buffer_length = htole32(len);
 154
 155         payload_offset = len;
 156         payload_free = MBIM_BUFFER_SIZE - (sizeof(*cmd) + len);
 157         payload_len = 0;
 158         payload_buffer = cmd->buffer;
 159
 160         return cmd->buffer;
 161 }
 162
 163 int
 164 mbim_send_command_msg(void)
 165 {
 166         struct command_message *cmd = (struct command_message *) mbim_buffer;
 167
 168         if (payload_len & 0x3) {
 169                 payload_len &= ~0x3;
 170                 payload_len += 4;
 171         }
 172
 173         cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len);
 174         mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length));
 175
 176         return mbim_send();
 177 }
 178
 179 int
 180 mbim_send_open_msg(void)
 181 {
 182         struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer;
 183
 184         mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg));
 185         msg->max_control_transfer = htole32(MBIM_BUFFER_SIZE);
 186
 187         return mbim_send();
 188 }
 189
 190 int
 191 mbim_send_close_msg(void)
 192 {
 193         struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
 194
 195         if (no_close) {
 196                 uloop_end();
 197                 return 0;
 198         }
 199         mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));
 200
 201         return mbim_send();
 202 }