Lots of changes for APDU handling and debugging. Thanks PCadach (bug #5265)!

git-svn-id: https://origsvn.digium.com/svn/libpri/trunk@286 2fbb986a-6c06-0410-b554-c9c1f0a7f128

pri_facility.c

@@ -32,6 +32,132 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <limits.h>
+
+static char *asn1id2text(int id)
+{
+	static char data[32];
+	static char *strings[] = {
+		"none",
+		"Boolean",
+		"Integer",
+		"Bit String",
+		"Octet String",
+		"NULL",
+		"Object Identifier",
+		"Object Descriptor",
+		"External Reference",
+		"Real Number",
+		"Enumerated",
+		"Embedded PDV",
+		"UTF-8 String",
+		"Relative Object ID",
+		"Reserved (0e)",
+		"Reserved (0f)",
+		"Sequence",
+		"Set",
+		"Numeric String",
+		"Printable String",
+		"Tele-Text String",
+		"IA-5 String",
+		"UTC Time",
+		"Generalized Time",
+	};
+	if (id > 0 && id <= 0x18) {
+		return strings[id];
+	} else {
+		sprintf(data, "Unknown (%02x)", id);
+		return data;
+	}
+}
+
+static int asn1_dumprecursive(struct pri *pri, void *comp_ptr, int len, int level)
+{
+	unsigned char *vdata = (unsigned char *)comp_ptr;
+	struct rose_component *comp;
+	int i = 0;
+	int j, k, l;
+	int clen = 0;
+
+	while (len > 0) {
+		GET_COMPONENT(comp, i, vdata, len);
+		pri_message(pri, "%*s%02X %04X", 2 * level, "", comp->type, comp->len);
+		if ((comp->type == 0) && (comp->len == 0))
+			return clen + 2;
+		if ((comp->type & ASN1_PC_MASK) == ASN1_PRIMITIVE) {
+			for (j = 0; j < comp->len; ++j)
+				pri_message(pri, " %02X", comp->data[j]);
+		}
+		if ((comp->type & ASN1_CLAN_MASK) == ASN1_UNIVERSAL) {
+			switch (comp->type & ASN1_TYPE_MASK) {
+			case 0:
+				pri_message(pri, " (none)");
+				break;
+			case ASN1_BOOLEAN:
+				pri_message(pri, " (BOOLEAN: %d)", comp->data[0]);
+				break;
+			case ASN1_INTEGER:
+				for (k = l = 0; k < comp->len; ++k)
+					l = (l << 8) | comp->data[k];
+				pri_message(pri, " (INTEGER: %d)", l);
+				break;
+			case ASN1_BITSTRING:
+				pri_message(pri, " (BITSTRING:");
+				for (k = 0; k < comp->len; ++k)
+				pri_message(pri, " %02x", comp->data[k]);
+				pri_message(pri, ")");
+				break;
+			case ASN1_OCTETSTRING:
+				pri_message(pri, " (OCTETSTRING:");
+				for (k = 0; k < comp->len; ++k)
+					pri_message(pri, " %02x", comp->data[k]);
+				pri_message(pri, ")");
+				break;
+			case ASN1_NULL:
+				pri_message(pri, " (NULL)");
+				break;
+			case ASN1_OBJECTIDENTIFIER:
+				pri_message(pri, " (OBJECTIDENTIFIER:");
+				for (k = 0; k < comp->len; ++k)
+					pri_message(pri, " %02x", comp->data[k]);
+				pri_message(pri, ")");
+				break;
+			case ASN1_ENUMERATED:
+				for (k = l = 0; k < comp->len; ++k)
+					l = (l << 8) | comp->data[k];
+				pri_message(pri, " (ENUMERATED: %d)", l);
+				break;
+			case ASN1_SEQUENCE:
+				pri_message(pri, " (SEQUENCE)");
+				break;
+			default:
+				pri_message(pri, " (component %02x - %s)", comp->type, asn1id2text(comp->type & ASN1_TYPE_MASK));
+				break;
+			}
+		}
+		else if ((comp->type & ASN1_CLAN_MASK) == ASN1_CONTEXT_SPECIFIC) {
+			pri_message(pri, " (CONTEXT SPECIFIC [%d])", comp->type & ASN1_TYPE_MASK);
+		}
+		else {
+			pri_message(pri, " (component %02x)", comp->type);
+		}
+		pri_message(pri, "\n");
+		if ((comp->type & ASN1_PC_MASK) == ASN1_CONSTRUCTOR)
+			j = asn1_dumprecursive(pri, comp->data, (comp->len ? comp->len : INT_MAX), level+1);
+		else
+			j = comp->len;
+		j += 2;
+		len -= j;
+		vdata += j;
+		clen += j;
+	}
+	return clen;
+}
+
+int asn1_dump(struct pri *pri, void *comp, int len)
+{
+	return asn1_dumprecursive(pri, comp, len, 0);
+}
 
 static unsigned char get_invokeid(struct pri *pri)
 {
@@ -46,28 +172,44 @@ struct addressingdataelements_presentednumberunscreened {
 	int  pres;
 };
 
+#define PRI_CHECKOVERFLOW(size) \
+		if (msgptr - message + (size) >= sizeof(message)) { \
+			*msgptr = '\0'; \
+			pri_message(pri, "%s", message); \
+			msgptr = message; \
+		}
+
 static void dump_apdu(struct pri *pri, unsigned char *c, int len) 
 {
 	#define MAX_APDU_LENGTH	255
+	static char hexs[16] = "0123456789ABCDEF";
 	int i;
 	char message[(2 + MAX_APDU_LENGTH * 3 + 6 + MAX_APDU_LENGTH + 3)] = "";	/* please adjust here, if you make changes below! */
+	char *msgptr;
 	
-	if (len > MAX_APDU_LENGTH)
+	msgptr = message;
-		return;
+	*msgptr++ = ' ';
-	
+	*msgptr++ = '[';
-	snprintf(message, sizeof(message)-1, " [");	
-	for (i=0; i<len; i++)
-		snprintf((char *)(message+strlen(message)), sizeof(message)-strlen(message)-1, " %02x", c[i]);
-	snprintf((char *)(message+strlen(message)), sizeof(message)-strlen(message)-1, " ] - [");
 	for (i=0; i<len; i++) {
-		if (c[i] < 20 || c[i] >= 128)
+		PRI_CHECKOVERFLOW(3);
-			snprintf((char *)(message+strlen(message)), sizeof(message)-strlen(message)-1, "<EFBFBD>");
+		*msgptr++ = ' ';
-		else
+		*msgptr++ = hexs[(c[i] >> 4) & 0x0f];
-			snprintf((char *)(message+strlen(message)), sizeof(message)-strlen(message)-1, "%c", c[i]);
+		*msgptr++ = hexs[(c[i]) & 0x0f];
 	}
-	snprintf((char *)(message+strlen(message)), sizeof(message)-strlen(message)-1, "]\n");
+	PRI_CHECKOVERFLOW(6);
-	pri_message(pri, message);
+	strcpy(msgptr, " ] - [");
+	msgptr += strlen(msgptr);
+	for (i=0; i<len; i++) {
+		PRI_CHECKOVERFLOW(1);
+		*msgptr++ = ((c[i] < ' ') || (c[i] > '~')) ? '.' : c[i];
+	}
+	PRI_CHECKOVERFLOW(2);
+	*msgptr++ = ']';
+	*msgptr++ = '\n';
+	*msgptr = '\0';
+	pri_message(pri, "%s", message);
 }
+#undef PRI_CHECKOVERFLOW
 
 int redirectingreason_from_q931(struct pri *pri, int redirectingreason)
 {
@@ -564,9 +706,11 @@ static int rose_diverting_leg_information2_encode(struct pri *pri, q931_call *ca
 	unsigned char buffer[256];
 	int len = 253;
 	
+#if 0	/* This is not required by specifications */
 	if (!strlen(call->callername)) {
 		return -1;
 	}
+#endif
 
 	buffer[i] = (ASN1_CONTEXT_SPECIFIC | Q932_PROTOCOL_EXTENSIONS);
 	i++;
@@ -749,7 +893,7 @@ static int add_callername_facility_ies(struct pri *pri, q931_call *c, int cpe)
 	}
 
 
-	/* Now the ADPu that contains the information that needs sent.
+	/* Now the APDU that contains the information that needs sent.
 	 * We can reuse the buffer since the queue function doesn't
 	 * need it. */
 
@@ -903,7 +1047,7 @@ extern int eect_initiate_transfer(struct pri *pri, q931_call *c1, q931_call *c2)
 
 	res = pri_call_apdu_queue(c1, Q931_FACILITY, buffer, i, NULL, NULL);
 	if (res) {
-		pri_message(pri, "Could not queue ADPU in facility message\n");
+		pri_message(pri, "Could not queue APDU in facility message\n");
 		return -1;
 	}
 
@@ -1110,7 +1254,7 @@ static int aoc_aoce_charging_unit_encode(struct pri *pri, q931_call *c, long cha
 	/* code below is untested */
 	res = pri_call_apdu_queue(c, Q931_FACILITY, buffer, i, NULL, NULL);
 	if (res) {
-		pri_message(pri, "Could not queue ADPU in facility message\n");
+		pri_message(pri, "Could not queue APDU in facility message\n");
 		return -1;
 	}
 
@@ -1305,6 +1449,7 @@ extern int pri_call_add_standard_apdus(struct pri *pri, q931_call *call)
 		return 0;
 	}
 
+#if 0
 	if (pri->localtype == PRI_NETWORK) {
 		switch (pri->switchtype) {
 			case PRI_SWITCH_NI2:
@@ -1324,6 +1469,10 @@ extern int pri_call_add_standard_apdus(struct pri *pri, q931_call *call)
 		}
 		return 0;
 	}
+#else
+	if (pri->switchtype == PRI_SWITCH_NI2)
+		add_callername_facility_ies(pri, call, (pri->localtype == PRI_CPE));
+#endif
 
 	return 0;
 }

pri_facility.h

@@ -146,7 +146,7 @@ struct rose_component {
 		break; \
 	(component) = (struct rose_component*)&((ptr)[idx]); \
 	if ((idx)+(component)->len+2 > (length)) { \
-		if ((component)->len != 128) \
+		if ((component)->len != ASN1_LEN_INDEF) \
 			pri_message(pri, "Length (%d) of 0x%X component is too long\n", (component)->len, (component)->type); \
 	}
 /*
@@ -169,6 +169,7 @@ struct rose_component {
 #define CHECK_COMPONENT(component, comptype, message) \
 	if ((component)->type && ((component)->type & ASN1_TYPE_MASK) != (comptype)) { \
 		pri_message(pri, (message), (component)->type); \
+		asn1_dump(pri, (component), (component)->len+2); \
 		break; \
 	}
 	
@@ -249,7 +250,7 @@ extern int mwi_message_send(struct pri *pri, q931_call *call, struct pri_sr *req
 /* starts a 2BCT */
 extern int eect_initiate_transfer(struct pri *pri, q931_call *c1, q931_call *c2);
 
-/* Use this function to queue a facility-IE born ADPU onto a call
+/* Use this function to queue a facility-IE born APDU onto a call
  * call is the call to use, messagetype is any one of the Q931 messages,
  * apdu is the apdu data, apdu_len is the length of the apdu data  */
 extern int pri_call_apdu_queue(q931_call *call, int messagetype, void *apdu, int apdu_len, void (*function)(void *data), void *data);
@@ -257,7 +258,9 @@ extern int pri_call_apdu_queue(q931_call *call, int messagetype, void *apdu, int
 /* Used by q931.c to cleanup the apdu queue upon destruction of a call */
 extern int pri_call_apdu_queue_cleanup(q931_call *call);
 
-/* Adds the "standard" ADPUs to a call */
+/* Adds the "standard" APDUs to a call */
 extern int pri_call_add_standard_apdus(struct pri *pri, q931_call *call);
 
+int asn1_dump(struct pri *pri, void *comp, int len);
+
 #endif /* _PRI_FACILITY_H */

pri_q931.h

@@ -243,6 +243,10 @@ typedef struct q931_ie {
 /* EuroISDN  */
 #define Q931_SENDING_COMPLETE		0xa1
 
+
+/* Q.SIG specific */
+#define QSIG_IE_TRANSIT_COUNT		0x31
+
 extern int q931_receive(struct pri *pri, q931_h *h, int len);
 
 extern int q931_alerting(struct pri *pri, q931_call *call, int channel, int info);

q931.c

@@ -1119,22 +1119,21 @@ static FUNC_SEND(transmit_facility)
 	int i = 0;
 
 	for (tmp = call->apdus; tmp; tmp = tmp->next) {
-		if (tmp->message == msgtype)
+		if ((tmp->message == msgtype) && !tmp->sent)
 			break;
 	}
 	
 	if (!tmp)	/* No APDU found */
 		return 0;
 
-	if (tmp->apdu_len > 235) { /* TODO: find out how much sapce we can use */
+	if (tmp->apdu_len > 235) { /* TODO: find out how much space we can use */
-		pri_message(pri, "Requested ADPU (%d bytes) is too long\n", tmp->apdu_len);
+		pri_message(pri, "Requested APDU (%d bytes) is too long\n", tmp->apdu_len);
-
-
 		return 0;
 	}
 	
-	memcpy(ie->data, tmp->apdu, tmp->apdu_len);
+	memcpy(&ie->data[i], tmp->apdu, tmp->apdu_len);
 	i += tmp->apdu_len;
+	tmp->sent = 1;
 
 	return i + 2;
 }
@@ -1434,57 +1433,71 @@ static FUNC_DUMP(dump_display)
 	}
 }
 
-static void dump_ie_data(unsigned char *c, int len)
+#define CHECK_OVERFLOW(limit) \
+	if (tmpptr - tmp + limit >= sizeof(tmp)) { \
+		*tmpptr = '\0'; \
+		pri_message(pri, "%s", tmpptr = tmp); \
+	}
+
+static void dump_ie_data(struct pri *pri, unsigned char *c, int len)
 {
-	char tmp[1024] = "";
+	static char hexs[16] = "0123456789ABCDEF";
-	int x=0;
+	char tmp[1024], *tmpptr;
 	int lastascii = 0;
-	while(len) {
+	tmpptr = tmp;
+	for (; len; --len, ++c) {
+		CHECK_OVERFLOW(7);
 		if (isprint(*c)) {
 			if (!lastascii) {
-				if (*tmp) { 
+				if (tmpptr != tmp) { 
-					tmp[x++] = ',';
+					*tmpptr++ = ',';
-					tmp[x++] = ' ';
+					*tmpptr++ = ' ';
 				}
-				tmp[x++] = '\'';
+				*tmpptr++ = '\'';
+				lastascii = 1;
 			}
-			tmp[x++] = *c;
+			*tmpptr++ = *c;
-			lastascii = 1;
 		} else {
 			if (lastascii) {
-				tmp[x++] = '\'';
+				*tmpptr++ = '\'';
+				lastascii = 0;
 			}
-			if (*tmp) { 
+			if (tmpptr != tmp) { 
-				tmp[x++] = ',';
+				*tmpptr++ = ',';
-				tmp[x++] = ' ';
+				*tmpptr++ = ' ';
 			}
-			sprintf (tmp + x, "0x%02x", *c);
+			*tmpptr++ = '0';
-			x += 4;
+			*tmpptr++ = 'x';
-			lastascii = 0;
+			*tmpptr++ = hexs[(*c >> 4) & 0x0f];
+			*tmpptr++ = hexs[(*c) & 0x0f];
 		}
-		c++;
-		len--;
 	}
 	if (lastascii)
-		tmp[x++] = '\'';
+		*tmpptr++ = '\'';
-	pri_message(NULL, tmp);
+	*tmpptr = '\0';
+	pri_message(pri, "%s", tmp);
 }
 
 static FUNC_DUMP(dump_facility)
 {
 	pri_message(pri, "%c Facility (len=%2d, codeset=%d) [ ", prefix, len, Q931_IE_CODESET(full_ie));
-	dump_ie_data(ie->data, ie->len);
+	dump_ie_data(pri, ie->data, ie->len);
 	pri_message(NULL, " ]\n");
+	if (ie->len > 1) {
+		pri_message(pri, "PROTOCOL %02X\n", ie->data[0] & ASN1_TYPE_MASK);
+		asn1_dump(pri, &ie->data[1], ie->len - 1);
+	}
+
 }
 
 static FUNC_DUMP(dump_network_spec_fac)
 {
 	pri_message(pri, "%c Network-Specific Facilities (len=%2d) [ ", prefix, ie->len);
 	if (ie->data[0] == 0x00) {
-		pri_message(pri, code2str(ie->data[1], facilities, sizeof(facilities) / sizeof(facilities[0])));
+ 		pri_message(pri, "%s", code2str(ie->data[1], facilities, sizeof(facilities) / sizeof(facilities[0])));
 	}
 	else
-		dump_ie_data(ie->data, ie->len);
+ 		dump_ie_data(pri, ie->data, ie->len);
 	pri_message(pri, " ]\n");
 }
 
@@ -1925,6 +1938,17 @@ static FUNC_DUMP(dump_signal)
 	pri_message(pri, "Signal %s (%d)\n", signal2str(ie->data[0]), ie->data[0]);
 }
 
+static FUNC_DUMP(dump_transit_count)
+{
+	/* Defined in ECMA-225 */
+	pri_message(pri, "%c Transit Count (len=%02d): ", prefix, len);
+	if (len < 3) {
+		pri_message(pri, "Invalid length\n");
+		return;
+	}
+	pri_message(pri, "Count=%d (0x%02x)\n", ie->data[0] & 0x1f, ie->data[0] & 0x1f);
+}
+
 
 struct ie ies[] = {
 	/* Codeset 0 - Common */
@@ -1954,7 +1978,7 @@ struct ie ies[] = {
 	{ 0, Q931_LOW_LAYER_COMPAT, "Low-layer Compatibility" },
 	{ 0, Q931_HIGH_LAYER_COMPAT, "High-layer Compatibility" },
 	{ 1, Q931_PACKET_SIZE, "Packet Size" },
-	{ 1, Q931_IE_FACILITY, "Facility" , dump_facility, receive_facility, transmit_facility },
+	{ 0, Q931_IE_FACILITY, "Facility" , dump_facility, receive_facility, transmit_facility },
 	{ 1, Q931_IE_REDIRECTION_NUMBER, "Redirection Number" },
 	{ 1, Q931_IE_REDIRECTION_SUBADDR, "Redirection Subaddress" },
 	{ 1, Q931_IE_FEATURE_ACTIVATE, "Feature Activation" },
@@ -1979,6 +2003,8 @@ struct ie ies[] = {
 	{ 1, Q931_IE_USER_USER_FACILITY, "User-User Facility" },
 	{ 1, Q931_IE_UPDATE, "Update" },
 	{ 1, Q931_SENDING_COMPLETE, "Sending Complete", dump_sending_complete, receive_sending_complete, transmit_sending_complete },
+	/* Codeset 4 - Q.SIG specific */
+	{ 1, QSIG_IE_TRANSIT_COUNT | Q931_CODESET(4), "Transit Count", dump_transit_count },
 	/* Codeset 6 - Network specific */
 	{ 1, Q931_IE_ORIGINATING_LINE_INFO, "Originating Line Information", dump_line_information, receive_line_information, transmit_line_information },
 	{ 1, Q931_IE_FACILITY | Q931_CODESET(6), "Facility", dump_facility, receive_facility, transmit_facility },
@@ -2090,14 +2116,14 @@ static inline void q931_dumpie(struct pri *pri, int codeset, q931_ie *ie, char p
 	int full_ie = Q931_FULL_IE(codeset, ie->ie);
 	int base_ie;
 
-	pri_message(NULL, "%c [", prefix);
+	pri_message(pri, "%c [", prefix);
-	pri_message(NULL, "%02x", ie->ie);
+	pri_message(pri, "%02x", ie->ie);
 	if (!(ie->ie & 0x80)) {
-		pri_message(NULL, " %02x", ielen(ie)-2);
+		pri_message(pri, " %02x", ielen(ie)-2);
 		for (x = 0; x + 2 < ielen(ie); ++x)
-			pri_message(NULL, " %02x", ie->data[x]);
+			pri_message(pri, " %02x", ie->data[x]);
 	}
-	pri_message(NULL, "]\n");
+	pri_message(pri, "]\n");
 
 	/* Special treatment for shifts */
 	if((full_ie & 0xf0) == Q931_LOCKING_SHIFT)
@@ -2114,7 +2140,7 @@ static inline void q931_dumpie(struct pri *pri, int codeset, q931_ie *ie, char p
 			return;
 		}
 	
-	pri_error(pri, "!! %c Unknown IE %d (len = %d)\n", prefix, base_ie, ielen(ie));
+	pri_error(pri, "!! %c Unknown IE %d (cs%d, len = %d)\n", prefix, Q931_IE_IE(base_ie), Q931_IE_CODESET(base_ie), ielen(ie));
 }
 
 static q931_call *q931_getcall(struct pri *pri, int cr)
@@ -2377,7 +2403,6 @@ static int send_message(struct pri *pri, q931_call *c, int msgtype, int ies[])
 	int offset=0;
 	int x;
 	int codeset;
-	struct apdu_event *facevent = c->apdus;
 	
 	memset(buf, 0, sizeof(buf));
 	len = sizeof(buf);
@@ -2386,24 +2411,7 @@ static int send_message(struct pri *pri, q931_call *c, int msgtype, int ies[])
 	x=0;
 	codeset = 0;
 	while(ies[x] > -1) {
-		if (ies[x] == Q931_IE_FACILITY) {
+		res = add_ie(pri, c, mh->msg, ies[x], (q931_ie *)(mh->data + offset), len, &codeset);
-			res = 0;
-			while (facevent) {
-				if (!facevent->sent && (facevent->message == msgtype)) { 
-					int tmpres;
-					tmpres = add_ie(pri, c, mh->msg, ies[x], (q931_ie *)(mh->data + offset), len, &codeset);
-					if (tmpres < 0) {
-						pri_error(pri, "!! Unable to add IE '%s'\n", ie2str(ies[x]));
-						return -1;
-					}
-					res += tmpres;
-					facevent->sent = 1;
-				}
-				facevent = facevent->next;
-			}
-		} else {
-			res = add_ie(pri, c, mh->msg, ies[x], (q931_ie *)(mh->data + offset), len, &codeset);
-		}
 
 		if (res < 0) {
 			pri_error(pri, "!! Unable to add IE '%s'\n", ie2str(ies[x]));

testprilib.c

@@ -147,7 +147,7 @@ static void event2(struct pri *pri, pri_event *e)
 	}
 }
 
-static void testmsg(char *s)
+static void testmsg(struct pri *pri, char *s)
 {
 	char *c;
 	static int keeplast = 0;
@@ -173,7 +173,7 @@ static void testmsg(char *s)
 		keeplast = 0;
 }
 
-static void testerr(char *s)
+static void testerr(struct pri *pri, char *s)
 {
 	char *c;
 	static int keeplast = 0;
@@ -264,6 +264,7 @@ int main(int argc, char *argv[])
 	}
 	first = pri;
 	pri_set_debug(pri, DEBUG_LEVEL);
+	pri_facility_enable(pri);
 	if (pthread_create(&tmp, NULL, dchan, pri)) {
 		perror("thread(0)");
 		exit(1);
@@ -273,6 +274,7 @@ int main(int argc, char *argv[])
 		exit(1);
 	}
 	pri_set_debug(pri, DEBUG_LEVEL);
+	pri_facility_enable(pri);
 	if (pthread_create(&tmp, NULL, dchan, pri)) {
 		perror("thread(1)");
 		exit(1);