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
 101 uint32_t
 102 mbim_get_int(void *buffer, uint32_t offset)
 103 {
 104         uint32_t *i = buffer + offset;
 105
 106         return le32toh(*i);
 107 }
 108
 109 const char*
 110 mbim_enum_string(struct mbim_enum *e, uint32_t key)
 111 {
 112         while (e->skey) {
 113                 if (key == e->key)
 114                         return e->val;
 115                 e++;
 116         }
 117         return NULL;
 118 }
 119
 120 void
 121 mbim_setup_header(struct mbim_message_header *hdr, MbimMessageType type, int length)
 122 {
 123         if (length < 16)
 124                 length = 16;
 125
 126         hdr->transaction_id = htole32(transaction_id++);
 127         hdr->type = htole32(type);
 128         hdr->length = htole32(length);
 129 }
 130
 131 uint8_t*
 132 mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int len)
 133 {
 134         struct command_message *cmd = (struct command_message *) mbim_buffer;
 135
 136         memset(mbim_buffer, 0, MBIM_BUFFER_SIZE);
 137
 138         cmd->fragment_header.total = htole32(1);
 139         cmd->fragment_header.current = htole32(0);
 140         memcpy(cmd->service_id, uuid, 16);
 141         cmd->command_id = htole32(command_id);
 142         cmd->command_type = htole32(type);
 143         cmd->buffer_length = htole32(len);
 144
 145         payload_offset = len;
 146         payload_free = MBIM_BUFFER_SIZE - (sizeof(*cmd) + len);
 147         payload_len = 0;
 148         payload_buffer = cmd->buffer;
 149
 150         return cmd->buffer;
 151 }
 152
 153 int
 154 mbim_send_command_msg(void)
 155 {
 156         struct command_message *cmd = (struct command_message *) mbim_buffer;
 157
 158         if (payload_len & 0x3) {
 159                 payload_len &= ~0x3;
 160                 payload_len += 4;
 161         }
 162
 163         cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len);
 164         mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length));
 165
 166         return mbim_send();
 167 }
 168
 169 int
 170 mbim_send_open_msg(void)
 171 {
 172         struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer;
 173
 174         mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg));
 175         msg->max_control_transfer = htole32(MBIM_BUFFER_SIZE);
 176
 177         return mbim_send();
 178 }
 179
 180 int
 181 mbim_send_close_msg(void)
 182 {
 183         struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
 184
 185         if (no_close) {
 186                 uloop_end();
 187                 return 0;
 188         }
 189         mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));
 190
 191         return mbim_send();
 192 }