2 files changed, 66 insertions, 0 deletions

diff --git a/src/smsutil.c b/src/smsutil.c
index 671ff710..7cf56de7 100644
--- a/src/smsutil.c
+++ b/src/smsutil.c
@@ -2484,3 +2484,48 @@ gboolean cbs_dcs_decode(guint8 dcs, gboolean *udhi, enum sms_class *cls,
 	return TRUE;
 }
 
+gboolean cbs_decode(const unsigned char *pdu, int len, struct cbs *out)
+{
+	/* CBS is always a fixed length of 88 bytes */
+	if (len != 88)
+		return FALSE;
+
+	out->gs = (enum cbs_geo_scope) ((pdu[0] >> 6) & 0x03);
+	out->message_code = ((pdu[0] & 0x3f) << 4) | ((pdu[1] >> 4) & 0xf);
+	out->update_number = (pdu[1] & 0xf);
+	out->message_identifier = (pdu[2] << 8) | pdu[3];
+	out->dcs = pdu[4];
+	out->max_pages = pdu[5] & 0xf;
+	out->page = (pdu[5] >> 4) & 0xf;
+
+	/* If a mobile receives the code 0000 in either the first field or
+	 * the second field then it shall treat the CBS message exactly the
+	 * same as a CBS message with page parameter 0001 0001 (i.e. a single
+	 * page message).
+	 */
+	if (out->max_pages == 0 || out->page == 0) {
+		out->max_pages = 1;
+		out->page = 1;
+	}
+
+	memcpy(out->ud, pdu + 6, 82);
+
+	return TRUE;
+}
+
+gboolean cbs_encode(const struct cbs *cbs, int *len, unsigned char *pdu)
+{
+	pdu[0] = (cbs->gs << 6) | ((cbs->message_code >> 4) & 0x3f);
+	pdu[1] = ((cbs->message_code & 0xf) << 4) | cbs->update_number;
+	pdu[2] = cbs->message_identifier >> 8;
+	pdu[3] = cbs->message_identifier & 0xff;
+	pdu[4] = cbs->dcs;
+	pdu[5] = cbs->max_pages | (cbs->page << 4);
+
+	memcpy(pdu + 6, cbs->ud, 82);
+
+	if (len)
+		*len = 88;
+
+	return TRUE;
+}
diff --git a/src/smsutil.h b/src/smsutil.h
index 5d647fdc..3e5f269e 100644
--- a/src/smsutil.h
+++ b/src/smsutil.h
@@ -201,6 +201,13 @@ enum cbs_language {
 	CBS_LANGUAGE_ICELANDIC = 0x24
 };
 
+enum cbs_geo_scope {
+	CBS_GEO_SCOPE_CELL_IMMEDIATE,
+	CBS_GEO_SCOPE_PLMN,
+	CBS_GEO_SCOPE_SERVICE_AREA,
+	CBS_GEO_SCOPE_CELL_NORMAL
+};
+
 struct sms_address {
 	enum sms_number_type number_type;
 	enum sms_numbering_plan numbering_plan;
@@ -356,6 +363,17 @@ struct sms_assembly {
 	GSList *assembly_list;
 };
 
+struct cbs {
+	enum cbs_geo_scope gs;			/* 2 bits */
+	guint16 message_code;			/* 10 bits */
+	guint8 update_number;			/* 4 bits */
+	guint16 message_identifier;		/* 16 bits */
+	guint8 dcs;				/* 8 bits */
+	guint8 max_pages;			/* 4 bits */
+	guint8 page;				/* 4 bits */
+	guint8 ud[82];
+};
+
 static inline gboolean is_bit_set(unsigned char oct, int bit)
 {
 	int mask = 0x1 << bit;
@@ -427,3 +445,6 @@ GSList *sms_text_prepare(const char *utf8, guint16 ref,
 gboolean cbs_dcs_decode(guint8 dcs, gboolean *udhi, enum sms_class *cls,
 			enum sms_charset *charset, gboolean *compressed,
 			enum cbs_language *language, gboolean *iso639);
+
+gboolean cbs_decode(const unsigned char *pdu, int len, struct cbs *out);
+gboolean cbs_encode(const struct cbs *cbs, int *len, unsigned char *pdu);