X-Git-Url: http://git.openwrt.org/?a=blobdiff_plain;f=mbim-msg.c;h=8f21aa90b49d0f1fc7885073523b3a30c59e6d00;hb=HEAD;hp=a327c6a191625cb68a48f984cf563626dca47fa7;hpb=d16d0d2a487687ff38c3b7b5fc530d660b1d85cc;p=project%2Fumbim.git

diff --git a/mbim-msg.c b/mbim-msg.c
index a327c6a..8f21aa9 100644
--- a/mbim-msg.c
+++ b/mbim-msg.c
@@ -53,8 +53,10 @@ mbim_add_payload(uint8_t len)
 int
 mbim_encode_string(struct mbim_string *str, char *in)
 {
-	int l = strlen(in);
-	int s = mbim_add_payload(l * 2);
+	const int l = strlen(in);
+	const int utf16_len = l * 2;
+	const int pad_len = utf16_len % 4;
+	const int s = mbim_add_payload(utf16_len + pad_len);
 	uint8_t *p = &payload_buffer[s];
 	int i;
 
@@ -62,19 +64,19 @@ mbim_encode_string(struct mbim_string *str, char *in)
 		return -1;
 
 	str->offset = htole32(s);
-	str->length = htole32(l * 2);
+	str->length = htole32(utf16_len);
+
 	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;
+	unsigned int i;
 
 	if (!le32toh(str->offset))
 		return NULL;
@@ -97,6 +99,25 @@ mbim_get_ipv4(void *buffer, char *out, uint32_t offset)
 	snprintf(out, 16, "%d.%d.%d.%d", b[0], b[1], b[2], b[3]);
 }
 
+void
+mbim_get_ipv6(void *buffer, char *out, uint32_t offset)
+{
+	uint8_t *b = buffer + offset;
+
+	snprintf(out, 40, "%x:%x:%x:%x:%x:%x:%x:%x", b[0] << 8 | b[1],
+		 b[2] << 8 | b[3], b[4] << 8 | b[5], b[6] << 8 | b[7],
+		 b[8] << 8 | b[9], b[10] << 8 | b[11], b[12] << 8 | b[13],
+		 b[14] << 8 | b[15]);
+}
+
+uint32_t
+mbim_get_int(void *buffer, uint32_t offset)
+{
+	uint32_t *i = buffer + offset;
+
+	return le32toh(*i);
+}
+
 const char*
 mbim_enum_string(struct mbim_enum *e, uint32_t key)
 {
@@ -124,7 +145,9 @@ mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int le
 {
 	struct command_message *cmd = (struct command_message *) mbim_buffer;
 
-	memset(mbim_buffer, 0, MBIM_BUFFER_SIZE);
+	if (!mbim_buffer)
+		return NULL;
+	memset(mbim_buffer, 0, mbim_bufsize);
 
 	cmd->fragment_header.total = htole32(1);
 	cmd->fragment_header.current = htole32(0);
@@ -134,7 +157,7 @@ mbim_setup_command_msg(uint8_t *uuid, uint32_t type, uint32_t command_id, int le
 	cmd->buffer_length = htole32(len);
 
 	payload_offset = len;
-	payload_free = MBIM_BUFFER_SIZE - (sizeof(*cmd) + len);
+	payload_free = mbim_bufsize - (sizeof(*cmd) + len);
 	payload_len = 0;
 	payload_buffer = cmd->buffer;
 
@@ -146,6 +169,8 @@ mbim_send_command_msg(void)
 {
 	struct command_message *cmd = (struct command_message *) mbim_buffer;
 
+	if (!mbim_buffer)
+		return 0;
 	if (payload_len & 0x3) {
 		payload_len &= ~0x3;
 		payload_len += 4;
@@ -163,7 +188,7 @@ 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);
+	msg->max_control_transfer = htole32(mbim_bufsize);
 
 	return mbim_send();
 }
@@ -173,8 +198,8 @@ mbim_send_close_msg(void)
 {
 	struct mbim_message_header *hdr = (struct mbim_message_header *) mbim_buffer;
 
-	if (no_close) {
-		uloop_end();
+	if (no_close || !mbim_buffer) {
+		mbim_end();
 		return 0;
 	}
 	mbim_setup_header(hdr, MBIM_MESSAGE_TYPE_CLOSE, sizeof(*hdr));