[PATCH 12/18] isimodem: wgmodem2.5 support in USSD
From: Jessica Nilsson <jessica.j.nilsson(a)stericsson.com>
---
drivers/isimodem/ussd.c | 36 ++++++++++++++++++++++++++++++------
1 files changed, 30 insertions(+), 6 deletions(-)
diff --git a/drivers/isimodem/ussd.c b/drivers/isimodem/ussd.c
index d649484..abc6e09 100644
--- a/drivers/isimodem/ussd.c
+++ b/drivers/isimodem/ussd.c
@@ -166,7 +166,11 @@ static void isi_request(struct ofono_ussd *ussd, int dcs,
{
struct ussd_data *ud = ofono_ussd_get_data(ussd);
struct isi_cb_data *cbd = isi_cb_data_new(ussd, cb, data);
- const uint8_t msg[] = {
+ const uint8_t padding_bytes[] = {0, 0, 0, 0, 0, 0, 0, 0};
+ GIsiModem *modem;
+ uint8_t iov_size = 2;
+
+ uint8_t msg[] = {
SS_GSM_USSD_SEND_REQ,
ud->mt_session
? SS_GSM_USSD_MT_REPLY
@@ -178,15 +182,34 @@ static void isi_request(struct ofono_ussd *ussd, int dcs,
len, /* string length */
/* USSD string goes here */
};
- const struct iovec iov[2] = {
- { (uint8_t *) msg, sizeof(msg) },
- { (uint8_t *) pdu, len }
- };
+ struct iovec iov[3] = { { 0, 0 }, { 0, 0 }, { 0, 0 } };
if (cbd == NULL || ud == NULL)
goto error;
- if (g_isi_client_vsend(ud->client, iov, 2,
+ modem = g_isi_client_modem(ud->client);
+
+ if (g_isi_modem_version_major(modem) == 2 &&
+ g_isi_modem_version_minor(modem) >= 0) {
+ uint8_t filled_len = (4 + len + 3) & ~3;
+ struct iovec t_iov[3] = {
+ { (uint8_t *) msg, sizeof(msg) },
+ { (uint8_t *)pdu, len },
+ { (uint8_t *) padding_bytes, (filled_len - len - 4) }
+ };
+ iov_size = 3;
+ memmove(iov, t_iov, sizeof(struct iovec) * iov_size);
+
+ msg[4] = filled_len;
+ } else {
+ struct iovec t_iov[2] = {
+ { (uint8_t *) msg, sizeof(msg) },
+ { (uint8_t *) pdu, len }
+ };
+ memmove(iov, t_iov, sizeof(struct iovec) * iov_size);
+ }
+
+ if (g_isi_client_vsend(ud->client, iov, iov_size,
ussd_send_resp_cb, cbd, g_free))
return;
@@ -241,6 +264,7 @@ static int isi_ussd_probe(struct ofono_ussd *ussd, unsigned int
vendor,
struct ussd_data *ud;
ud = g_try_new0(struct ussd_data, 1);
+
if (ud == NULL)
return -ENOMEM;
--
1.7.3.5