1 files changed, 169 insertions, 0 deletions
diff --git a/mbim-msg.c b/mbim-msg.c
new file mode 100644
index 0000000..88df1b6
--- /dev/null
+++ b/mbim-msg.c
@@ -0,0 +1,169 @@
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <alloca.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <libubox/utils.h>
+#include <libubox/uloop.h>
+
+#include "mbim.h"
+
+#include "data/mbim-service-basic-connect.h"
+
+int transaction_id = 1;
+uint8_t basic_connect[16] = { 0xa2, 0x89, 0xcc, 0x33, 0xbc, 0xbb, 0x8b, 0x4f,
+		     0xb6, 0xb0, 0x13, 0x3e, 0xc2, 0xaa, 0xe6,0xdf };
+static int payload_offset, payload_free, payload_len;
+static uint8_t *payload_buffer;
+
+int
+mbim_add_payload(uint8_t len)
+{
+	uint32_t offset = payload_offset;
+
+	if (payload_free < len)
+		return 0;
+
+	payload_free -= len;
+	payload_offset += len;
+	payload_len += len;
+
+	return offset;
+}
+
+int
+mbim_encode_string(struct mbim_string *str, char *in)
+{
+	int l = strlen(in);
+	int s = mbim_add_payload(l * 2);
+	uint8_t *p = &payload_buffer[s];
+	int i;
+
+	if (!s)
+		return -1;
+
+	str->offset = htole32(s);
+	str->length = htole32(l * 2);
+	for (i = 0; i < l; i++)
+		p[i * 2] = in[i];
+
+	return 0;
+}
+
+
+char *
+mbim_get_string(struct mbim_string *str, char *in)
+{
+	char *p = &in[le32toh(str->offset)];
+	int i;
+
+	if (!le32toh(str->offset))
+		return NULL;
+
+	if (le32toh(str->length)) {
+		for (i = 0; i < le32toh(str->length) / 2; i++)
+			p[i] = p[i * 2];
+		p[i] = '\0';
+		str->length = 0;
+	}
+
+	return p;
+}
+
+void
+mbim_get_ipv4(void *buffer, char *out, uint32_t offset)
+{
+	uint8_t *b = buffer + offset;
+
+	snprintf(out, 16, "%d.%d.%d.%d", b[0], b[1], b[2], b[3]);
+}
+
+const char*
+mbim_enum_string(struct mbim_enum *e, uint32_t key)
+{
+	while (e->skey) {
+		if (key == e->key)
+			return e->val;
+		e++;
+	}
+	return NULL;
+}
+
+void
+mbim_setup_header(struct mbim_message_header *hdr, MbimMessageType type, int length)
+{
+	if (length < 16)
+		length = 16;
+
+	hdr->transaction_id = htole32(transaction_id++);
+	hdr->type = htole32(type);
+	hdr->length = htole32(length);
+}
+
+uint8_t*
+mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int len)
+{
+	struct command_message *cmd = (struct command_message *) mbim_buffer;
+
+	memset(mbim_buffer, 0, MBIM_BUFFER_SIZE);
+
+	cmd->fragment_header.total = htole32(1);
+	cmd->fragment_header.current = htole32(0);
+	memcpy(cmd->service_id, uuid, 16);
+	cmd->command_id = htole32(command_id);
+	cmd->command_type = htole32(type);
+	cmd->buffer_length = htole32(len);
+
+	payload_offset = len;
+	payload_free = MBIM_BUFFER_SIZE - (sizeof(*cmd) + len);
+	payload_len = 0;
+	payload_buffer = cmd->buffer;
+
+	return cmd->buffer;
+}
+
+int
+mbim_send_command_msg(void)
+{
+	struct command_message *cmd = (struct command_message *) mbim_buffer;
+
+	if (payload_len & 0x3) {
+		payload_len &= ~0x3;
+		payload_len += 4;
+	}
+
+        cmd->buffer_length = htole32(le32toh(cmd->buffer_length) + payload_len);
+	mbim_setup_header(&cmd->header, MBIM_MESSAGE_TYPE_COMMAND, sizeof(*cmd) + le32toh(cmd->buffer_length));
+
+	return mbim_send();
+}
+
+int
+mbim_send_open_msg(void)
+{
+	struct mbim_open_message *msg = (struct mbim_open_message *) mbim_buffer;
+
+	mbim_setup_header(&msg->header, MBIM_MESSAGE_TYPE_OPEN, sizeof(*msg));
+	msg->max_control_transfer = htole32(MBIM_BUFFER_SIZE);
+
+	return mbim_send();
+}
+
+int
+mbim_send_close_msg(void)
+{
+	struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
+
+	if (no_close) {
+		uloop_end();
+		return 0;
+	}
+	mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));
+
+	return mbim_send();
+}