[PATCH 1/5] 23.032 GAD Shape structs and encoding utility.

[PATCH 1/5] 23.032 GAD Shape structs and encoding utility.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 14519 bytes --]

---
 Makefile.am   |    5 +-
 src/lcsutil.c |  426 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/lcsutil.h |   99 +++++++++++++
 3 files changed, 528 insertions(+), 2 deletions(-)
 create mode 100644 src/lcsutil.c
 create mode 100644 src/lcsutil.h

diff --git a/Makefile.am b/Makefile.am
index 2ca0703..2b59f01 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -269,9 +269,10 @@ src_ofonod_SOURCES = $(gdbus_sources) $(builtin_sources) \
 			src/storage.c src/cbs.c src/watch.c src/call-volume.c \
 			src/gprs.c src/idmap.h src/idmap.c \
 			src/radio-settings.c src/stkutil.h src/stkutil.c \
-			src/nettime.c
+			src/nettime.c src/lcsutil.c
 
-src_ofonod_LDADD = $(builtin_libadd) @GLIB_LIBS@ @DBUS_LIBS@ @CAPNG_LIBS@ -ldl
+src_ofonod_LDADD = $(builtin_libadd) @GLIB_LIBS@ @DBUS_LIBS@ @CAPNG_LIBS@ \
+			-ldl -lm
 
 src_ofonod_LDFLAGS = -Wl,--export-dynamic -Wl,--version-script=src/ofono.ver
 
diff --git a/src/lcsutil.c b/src/lcsutil.c
new file mode 100644
index 0000000..5add2e8
--- /dev/null
+++ b/src/lcsutil.c
@@ -0,0 +1,426 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2010  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdint.h>
+#include <math.h>
+
+#include <glib.h>
+
+#include <ofono/types.h>
+#include "lcsutil.h"
+
+/* 23.032 Section 6.1 */
+static ofono_bool_t gad_point_encode(uint8_t *buffer,
+					const struct gad_ellipsoid_point *pt)
+{
+	int val;
+
+	if (pt->lat < -90.0 || pt->lat > 90.0)
+		return FALSE;
+
+	if (pt->lat < 0.0)
+		buffer[0] = 0x80;
+	else
+		buffer[0] = 0x00;
+
+	val = fabs(pt->lat) * (1 << 23) / 90.0;
+	buffer[0] |= val >> 16;
+	buffer[1] = (val >> 8) & 0xff;
+	buffer[2] = (val >> 0) & 0xff;
+
+	val = pt->lon * (1 << 24) / 360.0;
+	buffer[3] = (val >> 16) & 0xff;
+	buffer[4] = (val >> 8) & 0xff;
+	buffer[5] = (val >> 0) & 0xff;
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.2 */
+static ofono_bool_t gad_uncertainty_encode(uint8_t *buffer, double r)
+{
+	if (r < 0.0)
+		return FALSE;
+
+	buffer[0] = MIN(127.0, round(log(r / 10.0 + 1) / log(1.1)));
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.3 */
+static ofono_bool_t gad_altitude_encode(uint8_t *buffer, double a)
+{
+	int val;
+
+	val = MIN(fabs@, 32767.0);
+	if (a >= 0.0)
+		buffer[0] = 0x00;
+	else
+		buffer[0] = 0x80;
+	buffer[0] |= val >> 8;
+	buffer[1] = val & 0xff;
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.4 */
+static ofono_bool_t gad_uncertainty_altitude_encode(uint8_t *buffer, double h)
+{
+	if (h < 0.0)
+		return FALSE;
+
+	buffer[0] = MIN(127.0, round(log(h / 45.0 + 1) / log(1.025)));
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.5 */
+static ofono_bool_t gad_confidence_encode(uint8_t *buffer, int p)
+{
+	if (p < 0 || p > 100)
+		return FALSE;
+
+	buffer[0] = p;
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.6 */
+static ofono_bool_t gad_radius_encode(uint8_t *buffer, double r)
+{
+	int val;
+
+	if (r < 0.0)
+		return FALSE;
+
+	val = MIN(r / 5.0, 65535.0);
+	buffer[0] |= val >> 8;
+	buffer[1] = val & 0xff;
+
+	return TRUE;
+}
+
+/* 23.032 Section 6.7 */
+static ofono_bool_t gad_offset_angle_encode(uint8_t *buffer, double ao)
+{
+	if (ao < 0.0 || ao >= 360.0)
+		return FALSE;
+
+	buffer[0] = ao / 2.0;
+
+	return TRUE;
+}
+
+static ofono_bool_t gad_include_angle_encode(uint8_t *buffer, double ai)
+{
+	if (ai <= 0.0 || ai > 360.0)
+		return FALSE;
+
+	buffer[0] = ceil(ai / 2.0);
+
+	return TRUE;
+}
+
+/* 23.032 Section 8.7 */
+static ofono_bool_t gad_horiz_speed_encode(uint8_t *buffer, double h)
+{
+	int val;
+
+	if (h < 0.0)
+		return FALSE;
+
+	val = MIN(MAX(0.0, h + 0.5), 65535.5);
+	buffer[0] = val >> 8;
+	buffer[1] = val & 0xff;
+
+	return TRUE;
+}
+
+/* 23.032 Section 8.8 */
+static ofono_bool_t gad_bearing_encode(uint8_t *buffer, double b)
+{
+	int val;
+
+	if (b < 0.0 || b >= 360.0)
+		return FALSE;
+
+	val = b;
+	buffer[0] |= val >> 8;
+	buffer[1] = val & 0xff;
+
+	return TRUE;
+}
+
+/* 23.032 Sections 8.9, 8.10 */
+static ofono_bool_t gad_vert_speed_encode(uint8_t *buffer, double v)
+{
+	buffer[0] = MIN(MAX(0.0, fabs(v) + 0.5), 255.5);
+
+	return TRUE;
+}
+
+/*
+ * Encode 'shape' into 'buffer'.  'len' is an in/out parameter that must
+ * initially contain the buffer capacity and receives the number of bytes
+ * written into the buffer.  Returns TRUE on success.
+ */
+ofono_bool_t gad_shape_encode(const struct gad_shape *shape,
+				uint8_t *buffer, int *len)
+{
+	int i;
+
+	if (*len < 7)
+		return FALSE;
+
+	buffer[0] = shape->type << 4;
+
+	switch (shape->type) {
+	case GAD_TYPE_ELLIPSOID_POINT:
+		*len = 7;
+
+		return gad_point_encode(buffer + 1, &shape->ellipsoid_point);
+
+	case GAD_TYPE_ELLIPSOID_CIRCLE:
+		if (*len < 8)
+			return FALSE;
+
+		if (gad_point_encode(buffer + 1,
+					&shape->ellipsoid_circle.origin) !=
+				TRUE)
+			return FALSE;
+
+		*len = 8;
+		return gad_uncertainty_encode(buffer + 7,
+						shape->ellipsoid_circle.r);
+
+	case GAD_TYPE_ELLIPSOID_ELLIPSE:
+		if (*len < 11)
+			return FALSE;
+
+		if (gad_point_encode(buffer + 1,
+					&shape->ellipsoid_ellipse.origin) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_encode(buffer + 7,
+						shape->ellipsoid_ellipse.r1) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_encode(buffer + 8,
+						shape->ellipsoid_ellipse.r2) !=
+				TRUE)
+			return FALSE;
+		if (gad_offset_angle_encode(buffer + 9,
+						shape->ellipsoid_ellipse.a) !=
+				TRUE)
+			return FALSE;
+		if (gad_confidence_encode(buffer + 10,
+					shape->ellipsoid_ellipse.confidence) !=
+				TRUE)
+			return FALSE;
+
+		*len = 11;
+		return TRUE;
+
+	case GAD_TYPE_POLYGON:
+		if (shape->polygon.count < 3 || shape->polygon.count > 15)
+			return FALSE;
+		if (*len < 1 + 6 * shape->polygon.count)
+			return FALSE;
+
+		buffer[0] |= shape->polygon.count;
+		for (i = 0; i < shape->polygon.count; i++)
+			if (gad_point_encode(buffer + i * 6 + 1,
+						&shape->polygon.point[i]) !=
+					TRUE)
+				return FALSE;
+
+		*len = 1 + 6 * shape->polygon.count;
+		return TRUE;
+
+	case GAD_TYPE_POINT_WITH_ALTITUDE:
+		if (*len < 9)
+			return FALSE;
+
+		if (gad_point_encode(buffer + 1,
+					&shape->point_with_altitude.origin) !=
+				TRUE)
+			return FALSE;
+		if (gad_altitude_encode(buffer + 7,
+					shape->point_with_altitude.altitude) !=
+				TRUE)
+			return FALSE;
+
+		*len = 9;
+		return TRUE;
+
+	case GAD_TYPE_ELLIPSOID_WITH_ALTITUDE:
+		if (*len < 14)
+			return FALSE;
+		if (shape->ellipsoid_with_altitude.r1 <
+				shape->ellipsoid_with_altitude.r2)
+			return FALSE;
+
+		if (gad_point_encode(buffer + 1,
+				&shape->ellipsoid_with_altitude.origin) !=
+				TRUE)
+			return FALSE;
+		if (gad_altitude_encode(buffer + 7,
+				shape->ellipsoid_with_altitude.altitude) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_encode(buffer + 9,
+					shape->ellipsoid_with_altitude.r1) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_encode(buffer + 10,
+					shape->ellipsoid_with_altitude.r2) !=
+				TRUE)
+			return FALSE;
+		if (gad_offset_angle_encode(buffer + 11,
+					shape->ellipsoid_with_altitude.a) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_altitude_encode(buffer + 12,
+					shape->ellipsoid_with_altitude.r3) !=
+				TRUE)
+			return FALSE;
+		if (gad_confidence_encode(buffer + 13,
+				shape->ellipsoid_with_altitude.confidence) !=
+				TRUE)
+			return FALSE;
+
+		*len = 14;
+		return TRUE;
+
+	case GAD_TYPE_ELLIPSOID_ARC:
+		if (*len < 13)
+			return FALSE;
+
+		if (gad_point_encode(buffer + 1,
+					&shape->ellipsoid_arc.origin) != TRUE)
+			return FALSE;
+		if (gad_radius_encode(buffer + 7, shape->ellipsoid_arc.r1) !=
+				TRUE)
+			return FALSE;
+		if (gad_uncertainty_encode(buffer + 9,
+						shape->ellipsoid_arc.r2) !=
+				TRUE)
+			return FALSE;
+		if (gad_offset_angle_encode(buffer + 10,
+						shape->ellipsoid_arc.alpha) !=
+				TRUE)
+			return FALSE;
+		if (gad_include_angle_encode(buffer + 11,
+						shape->ellipsoid_arc.beta) !=
+				TRUE)
+			return FALSE;
+		if (gad_confidence_encode(buffer + 12,
+					shape->ellipsoid_arc.confidence) !=
+				TRUE)
+			return FALSE;
+
+		*len = 13;
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+ofono_bool_t gad_velocity_encode(const struct gad_velocity *velocity,
+				uint8_t *buffer, int *len)
+{
+	if (velocity->has_vertical != TRUE &&
+			velocity->has_uncertainty != TRUE) {
+		if (*len < 4)
+			return FALSE;
+		buffer[0] = GAD_VELOCITY_TYPE_HORIZ << 4;
+
+		if (gad_bearing_encode(buffer + 0, velocity->bearing) != TRUE)
+			return FALSE;
+		if (gad_horiz_speed_encode(buffer + 2,
+						velocity->horizontal) != TRUE)
+			return FALSE;
+
+		*len = 4;
+		return TRUE;
+	} else if (velocity->has_vertical == TRUE &&
+			velocity->has_uncertainty != TRUE) {
+		if (*len < 5)
+			return FALSE;
+		buffer[0] = GAD_VELOCITY_TYPE_HORIZ_VERT << 4;
+		if (velocity->vertical < 0.0)
+			buffer[0] |= 1 << 1; /* Direction of Vert Speed bit */
+
+		if (gad_bearing_encode(buffer + 0, velocity->bearing) != TRUE)
+			return FALSE;
+		if (gad_horiz_speed_encode(buffer + 2,
+						velocity->horizontal) != TRUE)
+			return FALSE;
+		if (gad_vert_speed_encode(buffer + 4,
+						velocity->vertical) != TRUE)
+			return FALSE;
+
+		*len = 5;
+		return TRUE;
+	} else if (velocity->has_vertical != TRUE &&
+			velocity->has_uncertainty == TRUE) {
+		if (*len < 5)
+			return FALSE;
+		buffer[0] = GAD_VELOCITY_TYPE_HORIZ_UNCERTAINTY << 4;
+
+		if (gad_bearing_encode(buffer + 0, velocity->bearing) != TRUE)
+			return FALSE;
+		if (gad_horiz_speed_encode(buffer + 2,
+						velocity->horizontal) != TRUE)
+			return FALSE;
+		buffer[4] = velocity->horizontal_uncertainty;
+
+		*len = 5;
+		return TRUE;
+	} else if (velocity->has_vertical == TRUE &&
+			velocity->has_uncertainty == TRUE) {
+		if (*len < 7)
+			return FALSE;
+		buffer[0] = GAD_VELOCITY_TYPE_HORIZ_VERT_UNCERTAINTY << 4;
+		if (velocity->vertical < 0.0)
+			buffer[0] |= 1 << 1; /* Direction of Vert Speed bit */
+
+		if (gad_bearing_encode(buffer + 0, velocity->bearing) != TRUE)
+			return FALSE;
+		if (gad_horiz_speed_encode(buffer + 2,
+						velocity->horizontal) != TRUE)
+			return FALSE;
+		if (gad_vert_speed_encode(buffer + 4,
+						velocity->vertical) != TRUE)
+			return FALSE;
+		buffer[5] = velocity->horizontal_uncertainty;
+		buffer[6] = velocity->vertical_uncertainty;
+
+		*len = 7;
+		return TRUE;
+	}
+
+	return FALSE;
+}
diff --git a/src/lcsutil.h b/src/lcsutil.h
new file mode 100644
index 0000000..db560ef
--- /dev/null
+++ b/src/lcsutil.h
@@ -0,0 +1,99 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2010  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+enum gad_shape_type {
+	GAD_TYPE_ELLIPSOID_POINT		= 0,
+	GAD_TYPE_ELLIPSOID_CIRCLE		= 1,
+	GAD_TYPE_ELLIPSOID_ELLIPSE		= 3,
+	GAD_TYPE_POLYGON			= 5,
+	GAD_TYPE_POINT_WITH_ALTITUDE		= 8,
+	GAD_TYPE_ELLIPSOID_WITH_ALTITUDE	= 9,
+	GAD_TYPE_ELLIPSOID_ARC			= 10,
+};
+
+struct gad_ellipsoid_point {
+	double lat, lon;
+};
+
+struct gad_ellipsoid_circle {
+	struct gad_ellipsoid_point origin;
+	double r;
+};
+
+struct gad_ellipsoid_ellipse {
+	struct gad_ellipsoid_point origin;
+	double r1, r2, a;
+	int confidence;
+};
+
+struct gad_ellipsoid_polygon {
+	struct gad_ellipsoid_point point[15];
+	int count;
+};
+
+struct gad_point_with_altitude {
+	struct gad_ellipsoid_point origin;
+	double altitude;
+};
+
+struct gad_ellipsoid_with_altitude {
+	struct gad_ellipsoid_point origin;
+	double r1, r2, r3, a, altitude;
+	int confidence;
+};
+
+struct gad_ellipsoid_arc {
+	struct gad_ellipsoid_point origin;
+	double r1, r2, alpha, beta;
+	int confidence;
+};
+
+struct gad_shape {
+	enum gad_shape_type type;
+	union {
+		struct gad_ellipsoid_point ellipsoid_point;
+		struct gad_ellipsoid_circle ellipsoid_circle;
+		struct gad_ellipsoid_ellipse ellipsoid_ellipse;
+		struct gad_ellipsoid_polygon polygon;
+		struct gad_point_with_altitude point_with_altitude;
+		struct gad_ellipsoid_with_altitude ellipsoid_with_altitude;
+		struct gad_ellipsoid_arc ellipsoid_arc;
+	};
+};
+
+enum gad_velocity_type {
+	GAD_VELOCITY_TYPE_HORIZ				= 0,
+	GAD_VELOCITY_TYPE_HORIZ_VERT			= 1,
+	GAD_VELOCITY_TYPE_HORIZ_UNCERTAINTY		= 2,
+	GAD_VELOCITY_TYPE_HORIZ_VERT_UNCERTAINTY	= 3,
+};
+
+struct gad_velocity {
+	double horizontal, vertical, bearing;
+	uint8_t horizontal_uncertainty, vertical_uncertainty;
+	ofono_bool_t has_vertical;
+	ofono_bool_t has_uncertainty;
+};
+
+ofono_bool_t gad_shape_encode(const struct gad_shape *shape,
+				uint8_t *buffer, int *len);
+ofono_bool_t gad_velocity_encode(const struct gad_velocity *velocity,
+					uint8_t *buffer, int *len);
-- 
1.7.1.86.g0e460.dirty

[PATCH 2/5] GAD shape tests

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 5407 bytes --]

---
 Makefile.am         |    6 ++-
 unit/test-lcsutil.c |  158 +++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 163 insertions(+), 1 deletions(-)
 create mode 100644 unit/test-lcsutil.c

diff --git a/Makefile.am b/Makefile.am
index 2b59f01..a661200 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -360,7 +360,7 @@ unit_objects =
 noinst_PROGRAMS = unit/test-common unit/test-util unit/test-idmap \
 					unit/test-sms unit/test-simutil \
 					unit/test-mux unit/test-caif \
-					unit/test-stkutil
+					unit/test-stkutil unit/test-lcsutil
 
 unit_test_common_SOURCES = unit/test-common.c src/common.c
 unit_test_common_LDADD = @GLIB_LIBS@
@@ -399,6 +399,10 @@ unit_test_caif_SOURCES = unit/test-caif.c $(gatchat_sources) \
 unit_test_caif_LDADD = @GLIB_LIBS@
 unit_objects += $(unit_test_caif_OBJECTS)
 
+unit_test_lcsutil_SOURCES = unit/test-lcsutil.c src/lcsutil.c
+unit_test_lcsutil_LDADD = @GLIB_LIBS@ -lm
+unit_objects += $(unit_test_lcsutil_OBJECTS)
+
 noinst_PROGRAMS += gatchat/gsmdial gatchat/test-server gatchat/test-qcdm
 
 gatchat_gsmdial_SOURCES = gatchat/gsmdial.c $(gatchat_sources)
diff --git a/unit/test-lcsutil.c b/unit/test-lcsutil.c
new file mode 100644
index 0000000..ca494d6
--- /dev/null
+++ b/unit/test-lcsutil.c
@@ -0,0 +1,158 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2008-2010  Intel Corporation. All rights reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <string.h>
+#include <stdint.h>
+#include <glib.h>
+
+#include <ofono/types.h>
+
+#include "lcsutil.h"
+
+struct gad_shape_test {
+	uint8_t coding[128];
+	int len;
+	struct gad_shape shape;
+};
+
+static const struct gad_shape_test gad_shape_valid[] = {
+	{
+		.coding = { 0x00, 0x49, 0xf4, 0x9f, 0x0e, 0xee, 0xee, },
+		.len = 7,
+		.shape = {
+			.type = GAD_TYPE_ELLIPSOID_POINT,
+			{ .ellipsoid_point = { .lat = 52, .lon = 21 } },
+		},
+	}, {
+		.coding = {
+			0x90, 0x49, 0xf4, 0x9f, 0x0e, 0xee, 0xee,
+			0x80, 0x64, 0x3c, 0x02, 0x02, 0x3c, 0x5d,
+		},
+		.len = 14,
+		.shape = {
+			.type = GAD_TYPE_ELLIPSOID_WITH_ALTITUDE,
+			{ .ellipsoid_with_altitude = {
+				.origin = { .lat = 52, .lon = 21 }, /* deg */
+				.r1 = 3000.0, /* m */
+				.r2 = 2.1, /* m */
+				.r3 = 153.0, /* m */
+				.a = 5.0, /* deg */
+				.altitude = -100.0, /* m undeground */
+				.confidence = 93, /* % */
+			}},
+		},
+	}, {
+		.coding = {
+			0x55, 0x4a, 0x3d, 0x70, 0x0e, 0xee, 0xee,
+			0x49, 0x3e, 0x93, 0x00, 0x15, 0xd8, 0x40,
+			0xb6, 0x0b, 0xa8, 0xbf, 0x26, 0x91, 0x11,
+			0x11, 0xc9, 0x3e, 0x94, 0xa5, 0x43, 0x20,
+			0x13, 0xe9, 0x3e,
+		},
+		.len = 31,
+		.shape = {
+			.type = GAD_TYPE_POLYGON,
+			{ .polygon = {
+				.count = 5,
+				.point = {
+					{ .lat = 52.2, .lon = 21.0 },
+					{ .lat = 51.5, .lon = 0.12 },
+					{ .lat = 45.5, .lon = -122.7 },
+					{ .lat = -12.0, .lon = -77.0 },
+					{ .lat = -26.2, .lon = 28.0 },
+				},
+			}},
+		},
+	},
+
+	{},
+};
+
+static void test_gad_shape(void) {
+	const struct gad_shape_test *test;
+	uint8_t buf[128];
+	int len;
+
+	for (test = gad_shape_valid; test->len; test++) {
+		len = sizeof(buf);
+		g_assert(gad_shape_encode(&test->shape, buf, &len));
+		g_assert(len == test->len);
+		g_assert(memcmp(buf, test->coding, len) == 0);
+	}
+}
+
+struct gad_velocity_test {
+	uint8_t coding[128];
+	int len;
+	struct gad_velocity velocity;
+};
+
+static const struct gad_velocity_test gad_velocity_valid[] = {
+	{
+		.coding = { 0x33, 0x0e, 0x00, 0x1e, 0x05, 0x01, 0x01, },
+		.len = 7,
+		.velocity = {
+			.horizontal = 30.0, /* kmh */
+			.bearing = 270.0, /* deg */
+			.has_vertical = TRUE,
+			.vertical = -5, /* kmh */
+			.has_uncertainty = TRUE,
+			.horizontal_uncertainty = 1, /* kmh */
+			.vertical_uncertainty = 1, /* kmh */
+		},
+	}, {
+		.coding = { 0x00, 0x05, 0x00, 0x1e, },
+		.len = 4,
+		.velocity = {
+			.horizontal = 30.0, /* kmh */
+			.bearing = 5.0, /* deg */
+		},
+	},
+
+	{},
+};
+
+static void test_gad_velocity(void) {
+	const struct gad_velocity_test *test;
+	uint8_t buf[128];
+	int len;
+
+	for (test = gad_velocity_valid; test->len; test++) {
+		len = sizeof(buf);
+		g_assert(gad_velocity_encode(&test->velocity, buf, &len));
+		g_assert(len == test->len);
+		g_assert(memcmp(buf, test->coding, len) == 0);
+	}
+}
+
+int main(int argc, char **argv)
+{
+	g_test_init(&argc, &argv, NULL);
+
+	g_test_add_func("/testutil/GAD Shape", test_gad_shape);
+	g_test_add_func("/testutil/GAD Velocity", test_gad_velocity);
+
+	return g_test_run();
+}
-- 
1.7.1.86.g0e460.dirty

[PATCH 3/5] stkutil: Add the Geographical Location Reporting envelope builder

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 5932 bytes --]

---
 Makefile.am         |    4 +-
 src/stk.c           |    1 +
 src/stkutil.c       |   66 +++++++++++++++++++++++++++++++++++++++++++++++++++
 src/stkutil.h       |   15 +++++++++++
 unit/test-stkutil.c |    1 +
 5 files changed, 85 insertions(+), 2 deletions(-)

diff --git a/Makefile.am b/Makefile.am
index a661200..3784627 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -385,8 +385,8 @@ unit_objects += $(unit_test_simutil_OBJECTS)
 
 unit_test_stkutil_SOURCES = unit/test-stkutil.c src/util.c \
 				src/storage.c src/smsutil.c \
-				src/simutil.c src/stkutil.c
-unit_test_stkutil_LDADD = @GLIB_LIBS@
+				src/simutil.c src/stkutil.c src/lcsutil.c
+unit_test_stkutil_LDADD = @GLIB_LIBS@ -lm
 unit_objects += $(unit_test_stkutil_OBJECTS)
 
 unit_test_mux_SOURCES = unit/test-mux.c $(gatchat_sources)
diff --git a/src/stk.c b/src/stk.c
index b5c6919..2325326 100644
--- a/src/stk.c
+++ b/src/stk.c
@@ -35,6 +35,7 @@
 #include "ofono.h"
 
 #include "smsutil.h"
+#include "lcsutil.h"
 #include "stkutil.h"
 
 static GSList *g_drivers = NULL;
diff --git a/src/stkutil.c b/src/stkutil.c
index 44a8eff..914ac1f 100644
--- a/src/stkutil.c
+++ b/src/stkutil.c
@@ -31,6 +31,7 @@
 
 #include <ofono/types.h>
 #include "smsutil.h"
+#include "lcsutil.h"
 #include "stkutil.h"
 #include "simutil.h"
 #include "util.h"
@@ -5155,6 +5156,60 @@ static gboolean build_dataobj_rejection_cause_code(struct stk_tlv_builder *tlv,
 		stk_tlv_builder_close_container(tlv);
 }
 
+/* Described in TS 131.111 Section 8.95 */
+static gboolean build_dataobj_gad_shape(struct stk_tlv_builder *tlv,
+					const void *data, gboolean cr)
+{
+	const struct stk_gad_shapes *shapes = data;
+	unsigned char tag = STK_DATA_OBJECT_TYPE_GAD_SHAPE;
+	unsigned char shape_buf[128];
+	unsigned char velocity_buf[8];
+	int shape_len = sizeof(shape_buf);
+	int velocity_len = sizeof(velocity_buf);
+
+	if (shapes->has_shape != TRUE)
+		return TRUE;
+
+	if (gad_shape_encode(&shapes->shape, shape_buf, &shape_len) != TRUE)
+		return FALSE;
+
+	if (shapes->has_velocity == TRUE) {
+		if (gad_velocity_encode(&shapes->velocity,
+					velocity_buf, &velocity_len) != TRUE)
+			return FALSE;
+	} else
+		velocity_len = 0;
+
+	return stk_tlv_builder_open_container(tlv, cr, tag, FALSE) &&
+		stk_tlv_builder_append_byte(tlv, shape_len) &&
+		stk_tlv_builder_append_bytes(tlv, shape_buf, shape_len) &&
+		stk_tlv_builder_append_byte(tlv, velocity_len) &&
+		stk_tlv_builder_append_bytes(tlv, velocity_buf, velocity_len) &&
+		stk_tlv_builder_close_container(tlv);
+}
+
+/* Described in TS 131.111 Section 8.96 */
+static gboolean build_dataobj_nmea_sentence(struct stk_tlv_builder *tlv,
+					const void *data, gboolean cr)
+{
+	const char *nmea = data;
+	unsigned char tag = STK_DATA_OBJECT_TYPE_NMEA_SENTENCE;
+	int len;
+
+	if (nmea == NULL)
+		return TRUE;
+
+	len = strlen(nmea);
+	if (len > 0x7e)
+		return FALSE;
+
+	/* No need to convert because NMEA only uses basic ASCII, UTF8 subset */
+	return stk_tlv_builder_open_container(tlv, cr, tag, FALSE) &&
+		stk_tlv_builder_append_byte(tlv, len) &&
+		stk_tlv_builder_append_bytes(tlv, (void *) nmea, len) &&
+		stk_tlv_builder_close_container(tlv);
+}
+
 /* Described in TS 131.111 Section 8.98, 3GPP 24.301 Section 6.5.1 */
 static gboolean build_dataobj_eps_pdn_conn_params(struct stk_tlv_builder *tlv,
 						const void *data, gboolean cr)
@@ -5883,6 +5938,17 @@ const unsigned char *stk_pdu_from_envelope(const struct stk_envelope *envelope,
 	case STK_ENVELOPE_TYPE_TERMINAL_APP:
 		ok = build_envelope_terminal_apps(&builder, envelope);
 		break;
+	case STK_ENVELOPE_TYPE_GEOLOCATION_REPORT:
+		ok = build_dataobj(&builder,
+					build_envelope_dataobj_device_ids,
+					DATAOBJ_FLAG_CR,
+					envelope,
+					build_dataobj_gad_shape, 0,
+					&envelope->geolocation_report.shape,
+					build_dataobj_nmea_sentence, 0,
+					&envelope->geolocation_report.nmea,
+					NULL);
+		break;
 	default:
 		return NULL;
 	};
diff --git a/src/stkutil.h b/src/stkutil.h
index 471e10f..8141383 100644
--- a/src/stkutil.h
+++ b/src/stkutil.h
@@ -182,6 +182,7 @@ enum stk_data_object_type {
 	STK_DATA_OBJECT_TYPE_ROUTING_AREA_INFO =		0x73,
 	STK_DATA_OBJECT_TYPE_UPDATE_ATTACH_TYPE =		0x74,
 	STK_DATA_OBJECT_TYPE_REJECTION_CAUSE_CODE =		0x75,
+	STK_DATA_OBJECT_TYPE_GAD_SHAPE =			0x77,
 	STK_DATA_OBJECT_TYPE_NMEA_SENTENCE =			0x78,
 	STK_DATA_OBJECT_TYPE_PLMN_LIST =			0x79,
 	STK_DATA_OBJECT_TYPE_BROADCAST_NETWORK_INFO =		0x7A,
@@ -1031,6 +1032,14 @@ struct stk_routing_area_info {
 	unsigned char rac;
 };
 
+/* Defined in TS 131.111 Section 8.95 */
+struct stk_gad_shapes {
+	struct gad_shape shape;
+	struct gad_velocity velocity;
+	ofono_bool_t has_shape; /* Whether the struct is valid */
+	ofono_bool_t has_velocity; /* Whether velocity is known */
+};
+
 /* Defined in TS 131.111 Section 8.99 */
 struct stk_tracking_area_id {
 	char mnc[OFONO_MAX_MNC_LENGTH + 1];
@@ -1592,6 +1601,11 @@ struct stk_envelope_terminal_apps {
 	ofono_bool_t last;
 };
 
+struct stk_envelope_geolocation_report {
+	struct stk_gad_shapes shape;
+	const char *nmea;
+};
+
 struct stk_envelope {
 	enum stk_envelope_type type;
 	enum stk_device_identity_type src;
@@ -1608,6 +1622,7 @@ struct stk_envelope {
 		struct stk_envelope_mms_transfer_status mms_status;
 		struct stk_envelope_mms_notification_download mms_notification;
 		struct stk_envelope_terminal_apps terminal_apps;
+		struct stk_envelope_geolocation_report geolocation_report;
 	};
 };
 
diff --git a/unit/test-stkutil.c b/unit/test-stkutil.c
index 706dd85..0c38dab 100644
--- a/unit/test-stkutil.c
+++ b/unit/test-stkutil.c
@@ -34,6 +34,7 @@
 
 #include <ofono/types.h>
 #include "smsutil.h"
+#include "lcsutil.h"
 #include "stkutil.h"
 #include "util.h"
 
-- 
1.7.1.86.g0e460.dirty

[PATCH 4/5] stkutil: Add More Time terminal response builder

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 1013 bytes --]

---
 src/stkutil.c |    1 +
 src/stkutil.h |    1 +
 2 files changed, 2 insertions(+), 0 deletions(-)

diff --git a/src/stkutil.c b/src/stkutil.c
index 914ac1f..144251a 100644
--- a/src/stkutil.c
+++ b/src/stkutil.c
@@ -5504,6 +5504,7 @@ const unsigned char *stk_pdu_from_response(const struct stk_response *response,
 					&response->get_input.text,
 					NULL);
 		break;
+	case STK_COMMAND_TYPE_MORE_TIME:
 	case STK_COMMAND_TYPE_SEND_SMS:
 	case STK_COMMAND_TYPE_PLAY_TONE:
 		break;
diff --git a/src/stkutil.h b/src/stkutil.h
index 8141383..8f18de8 100644
--- a/src/stkutil.h
+++ b/src/stkutil.h
@@ -1438,6 +1438,7 @@ struct stk_response {
 		struct stk_response_generic display_text;
 		struct stk_response_get_inkey get_inkey;
 		struct stk_response_get_input get_input;
+		struct stk_response_generic more_time;
 		struct stk_response_generic play_tone;
 		struct stk_response_poll_interval poll_interval;
 		struct stk_response_generic refresh;
-- 
1.7.1.86.g0e460.dirty

Re: [PATCH 4/5] stkutil: Add More Time terminal response builder

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 186 bytes --]

Hi Andrew,

> ---
>  src/stkutil.c |    1 +
>  src/stkutil.h |    1 +
>  2 files changed, 2 insertions(+), 0 deletions(-)

This one has been applied, thanks.

Regards,
-Denis

[PATCH 5/5] test-stkutil: Add a More Time response builder test

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 1541 bytes --]

---
 unit/test-stkutil.c |   24 ++++++++++++++++++++++++
 1 files changed, 24 insertions(+), 0 deletions(-)

diff --git a/unit/test-stkutil.c b/unit/test-stkutil.c
index 0c38dab..31dc41e 100644
--- a/unit/test-stkutil.c
+++ b/unit/test-stkutil.c
@@ -16179,6 +16179,26 @@ static const struct terminal_response_test get_input_response_data_1221 = {
 	},
 };
 
+static const unsigned char more_time_response_111[] = {
+	0x81, 0x03, 0x01, 0x02, 0x00, 0x82, 0x02, 0x82,
+	0x81, 0x83, 0x01, 0x00,
+};
+
+static const struct terminal_response_test more_time_response_data_111 = {
+	.pdu = more_time_response_111,
+	.pdu_len = sizeof(more_time_response_111),
+	.response = {
+		.number = 1,
+		.type = STK_COMMAND_TYPE_MORE_TIME,
+		.qualifier = 0x00,
+		.src = STK_DEVICE_IDENTITY_TYPE_TERMINAL,
+		.dst = STK_DEVICE_IDENTITY_TYPE_UICC,
+		.result = {
+			.type = STK_RESULT_TYPE_SUCCESS,
+		},
+	},
+};
+
 static const unsigned char send_sms_response_111[] = {
 	0x81, 0x03, 0x01, 0x13, 0x00, 0x82, 0x02, 0x82,
 	0x81, 0x83, 0x01, 0x00,
@@ -21027,6 +21047,10 @@ int main(int argc, char **argv)
 	g_test_add_data_func("/teststk/More Time 1.1.1",
 				&more_time_data_111, test_more_time);
 
+	g_test_add_data_func("/teststk/More Time response 1.1.1",
+				&more_time_response_data_111,
+				test_terminal_response_encoding);
+
 	g_test_add_data_func("/teststk/Play Tone 1.1.1",
 				&play_tone_data_111, test_play_tone);
 	g_test_add_data_func("/teststk/Play Tone 1.1.2",
-- 
1.7.1.86.g0e460.dirty

Re: [PATCH 5/5] test-stkutil: Add a More Time response builder test

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 171 bytes --]

Hi Andrew,

> ---
>  unit/test-stkutil.c |   24 ++++++++++++++++++++++++
>  1 files changed, 24 insertions(+), 0 deletions(-)

Applied, thanks.

Regards,
-Denis

[PATCH 1/5] mbmmodem: Allow no response data for envelope.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 1243 bytes --]

Partially reverts
http://git.kernel.org/?p=network/ofono/ofono.git;a=commitdiff;h=f98c6dc91702c0d14c0afa2a4e32102d3105568d
---
 drivers/mbmmodem/stk.c |   12 +++++-------
 1 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/drivers/mbmmodem/stk.c b/drivers/mbmmodem/stk.c
index cea902e..77bd7b5 100644
--- a/drivers/mbmmodem/stk.c
+++ b/drivers/mbmmodem/stk.c
@@ -52,8 +52,8 @@ static void mbm_stke_cb(gboolean ok, GAtResult *result, gpointer user_data)
 	ofono_stk_envelope_cb_t cb = cbd->cb;
 	GAtResultIter iter;
 	struct ofono_error error;
-	const guint8 *pdu;
-	gint len;
+	const guint8 *pdu = NULL;
+	gint len = 0;
 
 	decode_at_error(&error, g_at_result_final_response(result));
 
@@ -64,11 +64,9 @@ static void mbm_stke_cb(gboolean ok, GAtResult *result, gpointer user_data)
 
 	g_at_result_iter_init(&iter, result);
 
-	if (g_at_result_iter_next(&iter, "*STKE:") == FALSE)
-		goto error;
-
-	if (g_at_result_iter_next_hexstring(&iter, &pdu, &len) == FALSE)
-		goto error;
+	if (g_at_result_iter_next(&iter, "*STKE:") == TRUE)
+		if (g_at_result_iter_next_hexstring(&iter, &pdu, &len) == FALSE)
+			goto error;
 
 	cb(&error, pdu, len, cbd->data);
 	return;
-- 
1.7.1.86.g0e460.dirty

Re: [PATCH 1/5] mbmmodem: Allow no response data for envelope.

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 343 bytes --]

Hi Andrew,

> Partially reverts
> http://git.kernel.org/?p=network/ofono/ofono.git;a=commitdiff;h=f98c6dc9170
> 2c0d14c0afa2a4e32102d3105568d ---
>  drivers/mbmmodem/stk.c |   12 +++++-------
>  1 files changed, 5 insertions(+), 7 deletions(-)
> 

The first four patches in this series have been applied, thanks.

Regards,
-Denis

[PATCH 2/5] mbm: Register stk driver post sim.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 824 bytes --]

---
 plugins/mbm.c |    3 ++-
 1 files changed, 2 insertions(+), 1 deletions(-)

diff --git a/plugins/mbm.c b/plugins/mbm.c
index 00b5550..9d059ef 100644
--- a/plugins/mbm.c
+++ b/plugins/mbm.c
@@ -318,7 +318,6 @@ static void mbm_pre_sim(struct ofono_modem *modem)
 
 	ofono_devinfo_create(modem, 0, "atmodem", data->modem_port);
 	sim = ofono_sim_create(modem, 0, "atmodem", data->modem_port);
-	ofono_stk_create(modem, 0, "mbmmodem", data->modem_port);
 
 	if (data->have_sim && sim)
 		ofono_sim_inserted_notify(sim, TRUE);
@@ -332,6 +331,8 @@ static void mbm_post_sim(struct ofono_modem *modem)
 
 	DBG("%p", modem);
 
+	ofono_stk_create(modem, 0, "mbmmodem", data->modem_port);
+
 	ofono_netreg_create(modem, OFONO_VENDOR_MBM, "atmodem",
 				data->modem_port);
 
-- 
1.7.1.86.g0e460.dirty

[PATCH 3/5] atmodem: Add PIN entry quirk for mbm.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 2638 bytes --]

Wait for *EPEV unsolicited response after PIN entered, otherwise the
next AT+CPIN? query still returns the old value for a fraction of a
second and ofono gets stuck until the next PIN entry attempt.
---
 drivers/atmodem/sim.c |   46 +++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 45 insertions(+), 1 deletions(-)

diff --git a/drivers/atmodem/sim.c b/drivers/atmodem/sim.c
index e722fbb..8d5a9fa 100644
--- a/drivers/atmodem/sim.c
+++ b/drivers/atmodem/sim.c
@@ -44,6 +44,7 @@
 struct sim_data {
 	GAtChat *chat;
 	unsigned int vendor;
+	guint epev_id;
 };
 
 static const char *crsm_prefix[] = { "+CRSM:", NULL };
@@ -509,6 +510,45 @@ error:
 	CALLBACK_WITH_FAILURE(cb, -1, data);
 }
 
+static void at_epev_notify(GAtResult *result, gpointer user_data)
+{
+	struct cb_data *cbd = user_data;
+	struct sim_data *sd = cbd->user;
+	ofono_sim_lock_unlock_cb_t cb = cbd->cb;
+	struct ofono_error error = { .type = OFONO_ERROR_TYPE_NO_ERROR };
+
+	cb(&error, cbd->data);
+
+	g_at_chat_unregister(sd->chat, sd->epev_id);
+	sd->epev_id = 0;
+}
+
+static void at_pin_send_cb(gboolean ok, GAtResult *result,
+				gpointer user_data)
+{
+	struct cb_data *cbd = user_data;
+	struct sim_data *sd = cbd->user;
+	ofono_sim_lock_unlock_cb_t cb = cbd->cb;
+	struct ofono_error error;
+
+	decode_at_error(&error, g_at_result_final_response(result));
+
+	/*
+	 * On the MBM modem, AT+CPIN? keeps returning SIM PIN for a moment
+	 * after successful AT+CPIN="..", but sends *EPEV when that changes.
+	 */
+	if (ok && sd->vendor == OFONO_VENDOR_MBM) {
+		sd->epev_id = g_at_chat_register(sd->chat, "*EPEV",
+							at_epev_notify,
+							FALSE, cbd, g_free);
+		return;
+	}
+
+	cb(&error, cbd->data);
+
+	g_free(cbd);
+}
+
 static void at_lock_unlock_cb(gboolean ok, GAtResult *result,
 				gpointer user_data)
 {
@@ -532,10 +572,12 @@ static void at_pin_send(struct ofono_sim *sim, const char *passwd,
 	if (!cbd)
 		goto error;
 
+	cbd->user = sd;
+
 	snprintf(buf, sizeof(buf), "AT+CPIN=\"%s\"", passwd);
 
 	ret = g_at_chat_send(sd->chat, buf, none_prefix,
-				at_lock_unlock_cb, cbd, g_free);
+				at_pin_send_cb, cbd, NULL);
 
 	memset(buf, 0, sizeof(buf));
 
@@ -740,6 +782,8 @@ static int at_sim_probe(struct ofono_sim *sim, unsigned int vendor,
 
 	if (sd->vendor == OFONO_VENDOR_WAVECOM)
 		g_at_chat_add_terminator(chat, "+CPIN:", 6, TRUE);
+	if (sd->vendor == OFONO_VENDOR_MBM)
+		g_at_chat_send(chat, "AT*EPEE=1", NULL, NULL, NULL, NULL);
 
 	ofono_sim_set_data(sim, sd);
 	g_idle_add(at_sim_register, sim);
-- 
1.7.1.86.g0e460.dirty

[PATCH 4/5] mbm: Use MBM vendor quirk of the sim driver.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 620 bytes --]

---
 plugins/mbm.c |    3 ++-
 1 files changed, 2 insertions(+), 1 deletions(-)

diff --git a/plugins/mbm.c b/plugins/mbm.c
index 9d059ef..9193539 100644
--- a/plugins/mbm.c
+++ b/plugins/mbm.c
@@ -317,7 +317,8 @@ static void mbm_pre_sim(struct ofono_modem *modem)
 	DBG("%p", modem);
 
 	ofono_devinfo_create(modem, 0, "atmodem", data->modem_port);
-	sim = ofono_sim_create(modem, 0, "atmodem", data->modem_port);
+	sim = ofono_sim_create(modem, OFONO_VENDOR_MBM, "atmodem",
+				data->modem_port);
 
 	if (data->have_sim && sim)
 		ofono_sim_inserted_notify(sim, TRUE);
-- 
1.7.1.86.g0e460.dirty

[PATCH 5/5] mbm: Move +CFUN=1 to after SIM authentication.

[Andrzej Zaborowski]

[-- Attachment #1: Type: text/plain, Size: 3853 bytes --]

This is mostly a hack, as discussed on IRC we don't know what the
"correct" order is, but this seems to make the SIM toolkit work
more or less reliably on this modem, on SIMs with PIN enabled or
disabled alike.  Use this if you want to try out the STK menu.

In particular I'm not sure if the modem will be able to make
emergency calls unauthenticated, without CFUN=1.

I've tried other things, but generally, without this patch, with
PIN disabled in the SIM, the initial proactive command is only
passed through by the modem if the profile download is done before
any of the atom drivers initialise, (before sim driver).  With PIN
enabled it won't work at all.

Incidentally it seems that on the calypso we also need to send
+CFUN=1 only after SIM authentication, for things to work.

With this patch, the initial proactive command still only works in
the first ofono session after modem reset.  When ofono restarts
without unplugging or resetting (AT*E2RESET) the modem, on the
next session the SIM doesn't get reinitialised and will not send
the Set Up Menu unless it's one of those SIMs that resend it
periodically.
---
 plugins/mbm.c |   55 ++++++++++++++++++++-----------------------------------
 1 files changed, 20 insertions(+), 35 deletions(-)

diff --git a/plugins/mbm.c b/plugins/mbm.c
index 9193539..7873f54 100644
--- a/plugins/mbm.c
+++ b/plugins/mbm.c
@@ -121,22 +121,6 @@ poweron:
 	ofono_modem_set_powered(modem, TRUE);
 }
 
-static void cfun_enable(gboolean ok, GAtResult *result, gpointer user_data)
-{
-	struct ofono_modem *modem = user_data;
-	struct mbm_data *data = ofono_modem_get_data(modem);
-
-	DBG("");
-
-	if (!ok) {
-		ofono_modem_set_powered(modem, FALSE);
-		return;
-	}
-
-	g_at_chat_send(data->modem_port, "AT+CRSM=242", crsm_prefix,
-			status_check, modem, NULL);
-}
-
 static void cfun_query(gboolean ok, GAtResult *result, gpointer user_data)
 {
 	struct ofono_modem *modem = user_data;
@@ -146,23 +130,13 @@ static void cfun_query(gboolean ok, GAtResult *result, gpointer user_data)
 
 	DBG("%d", ok);
 
-	if (!ok)
-		return;
-
-	g_at_result_iter_init(&iter, result);
-
-	if (g_at_result_iter_next(&iter, "+CFUN:") == FALSE)
-		return;
-
-	g_at_result_iter_next_number(&iter, &status);
-
-	if (status == 4) {
-		g_at_chat_send(data->modem_port, "AT+CFUN=1", none_prefix,
-				cfun_enable, modem, NULL);
+	if (!ok) {
+		ofono_modem_set_powered(modem, FALSE);
 		return;
 	}
 
-	cfun_enable(TRUE, NULL, modem);
+	g_at_chat_send(data->modem_port, "AT+CRSM=242", crsm_prefix,
+			status_check, modem, NULL);
 }
 
 static void emrdy_notifier(GAtResult *result, gpointer user_data)
@@ -324,16 +298,14 @@ static void mbm_pre_sim(struct ofono_modem *modem)
 		ofono_sim_inserted_notify(sim, TRUE);
 }
 
-static void mbm_post_sim(struct ofono_modem *modem)
+static void cfun_enable(gboolean ok, GAtResult *result,
+					gpointer user_data)
 {
+	struct ofono_modem *modem = user_data;
 	struct mbm_data *data = ofono_modem_get_data(modem);
 	struct ofono_gprs *gprs;
 	struct ofono_gprs_context *gc;
 
-	DBG("%p", modem);
-
-	ofono_stk_create(modem, 0, "mbmmodem", data->modem_port);
-
 	ofono_netreg_create(modem, OFONO_VENDOR_MBM, "atmodem",
 				data->modem_port);
 
@@ -348,6 +320,19 @@ static void mbm_post_sim(struct ofono_modem *modem)
 		ofono_gprs_add_context(gprs, gc);
 }
 
+static void mbm_post_sim(struct ofono_modem *modem)
+{
+	struct mbm_data *data = ofono_modem_get_data(modem);
+
+	DBG("%p", modem);
+
+	ofono_stk_create(modem, 0, "mbmmodem", data->modem_port);
+
+	g_at_chat_send(data->modem_port, "AT+CFUN=1", none_prefix,
+			cfun_enable, modem, NULL);
+
+}
+
 static struct ofono_modem_driver mbm_driver = {
 	.name		= "mbm",
 	.probe		= mbm_probe,
-- 
1.7.1.86.g0e460.dirty