mbim-msg.c

   1 #include <sys/types.h>
   2 #include <sys/stat.h>
   3
   4 #include <alloca.h>
   5 #include <fcntl.h>
   6 #include <stdio.h>
   7 #include <stdlib.h>
   8 #include <string.h>
   9 #include <unistd.h>
  10
  11 #include <libubox/utils.h>
  12 #include <libubox/uloop.h>
  13
  14 #include "mbim.h"
  15
  16 #include "data/mbim-service-basic-connect.h"
  17
  18 int transaction_id = 1;
  19 uint8_t basic_connect[16] = { 0xa2, 0x89, 0xcc, 0x33, 0xbc, 0xbb, 0x8b, 0x4f,
  20                      0xb6, 0xb0, 0x13, 0x3e, 0xc2, 0xaa, 0xe6,0xdf };
  21 static int payload_offset, payload_free, payload_len;
  22 static uint8_t *payload_buffer;
  23
  24 int
  25 mbim_add_payload(uint8_t len)
  26 {
  27         uint32_t offset = payload_offset;
  28
  29         if (payload_free < len)
  30                 return 0;
  31
  32         payload_free -= len;
  33         payload_offset += len;
  34         payload_len += len;
  35
  36         return offset;
  37 }
  38
  39 int
  40 mbim_encode_string(struct mbim_string *str, char *in)
  41 {
  42         int l = strlen(in);
  43         int s = mbim_add_payload(l * 2);
  44         uint8_t *p = &payload_buffer[s];
  45         int i;
  46
  47         if (!s)
  48                 return -1;
  49
  50         str->offset = htole32(s);
  51         str->length = htole32(l * 2);
  52         for (i = 0; i < l; i++)
  53                 p[i * 2] = in[i];
  54
  55         return 0;
  56 }
  57
  58
  59 char *
  60 mbim_get_string(struct mbim_string *str, char *in)
  61 {
  62         char *p = &in[le32toh(str->offset)];
  63         int i;
  64
  65         if (!le32toh(str->offset))
  66                 return NULL;
  67
  68         if (le32toh(str->length)) {
  69                 for (i = 0; i < le32toh(str->length) / 2; i++)
  70                         p[i] = p[i * 2];
  71                 p[i] = '\0';
  72                 str->length = 0;
  73         }
  74
  75         return p;
  76 }
  77
  78 void
  79 mbim_get_ipv4(void *buffer, char *out, uint32_t offset)
  80 {
  81         uint8_t *b = buffer + offset;
  82
  83         snprintf(out, 16, "%d.%d.%d.%d", b[0], b[1], b[2], b[3]);
  84 }
  85
  86 const char*
  87 mbim_enum_string(struct mbim_enum *e, uint32_t key)
  88 {
  89         while (e->skey) {
  90                 if (key == e->key)
  91                         return e->val;
  92                 e++;
  93         }
  94         return NULL;
  95 }
  96
  97 void
  98 mbim_setup_header(struct mbim_message_header *hdr, MbimMessageType type, int length)
  99 {
 100         if (length < 16)
 101                 length = 16;
 102
 103         hdr->transaction_id = htole32(transaction_id++);
 104         hdr->type = htole32(type);
 105         hdr->length = htole32(length);
 106 }
 107
 108 uint8_t*
 109 mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int len)
 110 {
 111         struct command_message *cmd = (struct command_message *) mbim_buffer;
 112
 113         memset(mbim_buffer, 0, MBIM_BUFFER_SIZE);
 114
 115         cmd->fragment_header.total = htole32(1);
 116         cmd->fragment_header.current = htole32(0);
 117         memcpy(cmd->service_id, uuid, 16);
 118         cmd->command_id = htole32(command_id);
 119         cmd->command_type = htole32(type);
 120         cmd->buffer_length = htole32(len);
 121
 122         payload_offset = len;
 123         payload_free = MBIM_BUFFER_SIZE - (sizeof(*cmd) + len);
 124         payload_len = 0;
 125         payload_buffer = cmd->buffer;
 126
 127         return cmd->buffer;
 128 }
 129
 130 int
 131 mbim_send_command_msg(void)
 132 {
 133         struct command_message *cmd = (struct command_message *) mbim_buffer;
 134
 135         if (payload_len & 0x3) {
 136                 payload_len &= ~0x3;
 137                 payload_len += 4;
 138         }
 139
 140         cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len);
 141         mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length));
 142
 143         return mbim_send();
 144 }
 145
 146 int
 147 mbim_send_open_msg(void)
 148 {
 149         struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer;
 150
 151         mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg));
 152         msg->max_control_transfer = htole32(MBIM_BUFFER_SIZE);
 153
 154         return mbim_send();
 155 }
 156
 157 int
 158 mbim_send_close_msg(void)
 159 {
 160         struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
 161
 162         if (no_close) {
 163                 uloop_end();
 164                 return 0;
 165         }
 166         mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));
 167
 168         return mbim_send();
 169 }