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