modules/sipt: ss7 messages are now modified in place

 - lumps are now used to modify individual ss7 headers instead
   of rebuilding the full body from scratch.  This should result
   in a performance increase

modules/sipt/README

@@ -71,8 +71,7 @@ Chapter 1. Admin Guide
    Module for updating ISUP encapuslated in SIP (SIP-T/SIP-I)
 
    The sipt module can be used to update various ss7 headers contained
-   inside a message. If you want to call modify methods multiple times, be
-   sure to call msg_apply_changes from the textopsx package
+   inside a message.
 
 2. Dependencies
 

modules/sipt/doc/sipt_admin.xml

@@ -19,8 +19,7 @@
 	<para>Module for updating ISUP encapuslated in SIP (SIP-T/SIP-I)</para>
 	<para>
 	The sipt module can be used to update various ss7 headers contained inside
-	a message.  If you want to call modify methods multiple times, be sure to call
-	msg_apply_changes from the textopsx package
+	a message. 
 	</para>
 	</section>
 	<section>

modules/sipt/sdp_mangle.c

@@ -0,0 +1,73 @@
+/*
+ *
+ * Copyright (C) 2013 Voxbone SA
+ *
+ * This file is part of SIP-Router, a free SIP server.
+ *
+ * SIP-Router is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version
+ *
+ * SIP-Router 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * 
+ */
+
+#include "sdp_mangle.h"
+
+int replace_body_segment(struct sdp_mangler * mangler, int offset, int len, unsigned char * new_data, int new_len)
+{
+	
+	struct lump * l;
+	char *s;
+
+	l = del_lump(mangler->msg, mangler->body_offset + offset, len, 0);
+
+	if(l == NULL)
+	{
+		return -1;
+	}
+
+ 	s = pkg_malloc(new_len);
+	memcpy(s, new_data, new_len);
+
+	if(insert_new_lump_after(l, s, new_len, 0) == 0)
+	{
+		pkg_free(s);
+		return -2;
+	}
+
+	return 0;
+}
+
+int add_body_segment(struct sdp_mangler * mangler, int offset, unsigned char * new_data, int new_len)
+{
+	
+	struct lump * l;
+	char *s;
+	int exists;
+	l = anchor_lump2(mangler->msg, mangler->body_offset + offset, 0, 0, &exists);
+	if(l == NULL)
+	{
+		return -1;
+	}
+
+ 	s = pkg_malloc(new_len);
+	memcpy(s, new_data, new_len);
+	if(insert_new_lump_after(l, s, new_len, 0) == 0)
+	{
+		pkg_free(s);
+		return -2;
+	}
+
+	return 0;
+}
+

modules/sipt/sdp_mangle.h

@@ -0,0 +1,41 @@
+/*
+ *
+ * Copyright (C) 2013 Voxbone SA
+ *
+ * This file is part of SIP-Router, a free SIP server.
+ *
+ * SIP-Router is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version
+ *
+ * SIP-Router 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ * 
+ */
+
+#include "../../data_lump.h"
+#include "../../parser/msg_parser.h"	/* struct sip_msg */
+
+#ifndef _SIPT_SDP_MANGLE_
+#define _SIPT_SDP_MANGLE_
+
+
+struct sdp_mangler
+{
+	struct sip_msg *msg;
+	int body_offset;
+};
+
+
+int replace_body_segment(struct sdp_mangler * mangler, int offset, int len, unsigned char * new_data, int new_len);
+int add_body_segment(struct sdp_mangler * mangler, int offset, unsigned char * new_data, int new_len);
+
+#endif

modules/sipt/sipt.c

@@ -182,100 +182,6 @@ static inline int get_boundary_param(struct sip_msg *msg, str *boundary)
         return 0;
 }
 
-static char * SDP_HEADER = "Content-Type: application/sdp\r\n\r\n";
-static char * ISUP_HEADER = "Content-Type: application/ISUP; version=ITU-93\r\nContent-Disposition: signal; handling=optional\r\n\r\n";
-
-static int replace_body(struct sip_msg *msg, str * nb)
-{
-	str body;
-        body.s = get_body(msg);
-	struct lump *anchor;
-	char * buf;
-	free_lump_list(msg->body_lumps);
-	msg->body_lumps = NULL;
-
-
-        if (msg->content_length)
-        {
-                body.len = get_content_length( msg );
-                if(body.len > 0)
-                {
-                        if(body.s+body.len>msg->buf+msg->len)
-                        {
-                                LM_ERR("invalid content length: %d\n", body.len);
-                                return -1;
-                        }
-                        if(del_lump(msg, body.s - msg->buf, body.len, 0) == 0)
-                        {
-                                LM_ERR("cannot delete existing body");
-                                return -1;
-                        }
-                }
-        }
-
-        anchor = anchor_lump(msg, msg->unparsed - msg->buf, 0, 0);
-
-        if (anchor == 0)
-        {
-                LM_ERR("failed to get anchor\n");
-                return -1;
-        }
-
-        if (msg->content_length==0)
-        {
-                /* need to add Content-Length */
-                int len = nb->len;
-		int value_len;
-                char* value_s=int2str(len, &value_len);
-                LM_DBG("content-length: %d (%s)\n", value_len, value_s);
-
-                len=CONTENT_LENGTH_LEN+value_len+CRLF_LEN;
-                buf=pkg_malloc(sizeof(char)*(len));
-
-                if (buf==0)
-                {
-                        LM_ERR("out of pkg memory\n");
-                        return -1;
-                }
-
-                memcpy(buf, CONTENT_LENGTH, CONTENT_LENGTH_LEN);
-                memcpy(buf+CONTENT_LENGTH_LEN, value_s, value_len);
-                memcpy(buf+CONTENT_LENGTH_LEN+value_len, CRLF, CRLF_LEN);
-                if (insert_new_lump_after(anchor, buf, len, 0) == 0)
-                {
-                        LM_ERR("failed to insert content-length lump\n");
-                        pkg_free(buf);
-                        return -1;
-                }
-        }
-
-
-        anchor = anchor_lump(msg, body.s - msg->buf, 0, 0);
-
-        if (anchor == 0)
-        {
-                LM_ERR("failed to get body anchor\n");
-                return -1;
-        }
-
-        buf=pkg_malloc(sizeof(char)*(nb->len));
-        if (buf==0)
-        {
-                LM_ERR("out of pkg memory\n");
-                return -1;
-        }
-        memcpy(buf, nb->s, nb->len);
-        if (insert_new_lump_after(anchor, buf, nb->len, 0) == 0)
-        {
-                LM_ERR("failed to insert body lump\n");
-                pkg_free(buf);
-                return -1;
-        }
-        LM_DBG("new body: [%.*s]", nb->len, nb->s);
-
-	return 0;
-}
-
 static int sipt_get_hop_counter(struct sip_msg *msg, pv_param_t *param, pv_value_t *res)
 {
 	str body;
@@ -412,6 +318,7 @@ static int sipt_destination(struct sip_msg *msg, char *_destination, char *_hops
 	unsigned int int_nai = 0;
 	str2int(nai, &int_nai);
 	str * destination = (str*)_destination;
+	struct sdp_mangler mangle;
 
 	// update forwarded iam
 	str body;
@@ -443,92 +350,24 @@ static int sipt_destination(struct sip_msg *msg, char *_destination, char *_hops
 		return -1;
 	}
 
+	mangle.msg = msg;
+	mangle.body_offset = (int)(body.s - msg->buf);
 
 
-	unsigned int offset = 0;
-
-	if(sdp.s != NULL)
-	{
-		// we need to be clean, handle 2 cases
-		// one with sdp, one without sdp
-		str boundary = {0,0}; 
-		get_boundary_param(msg, &boundary);
-		memcpy(newbuf+offset, "--", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,SDP_HEADER, strlen(SDP_HEADER));
-		offset += strlen(SDP_HEADER);
-
-
-		memcpy(newbuf+offset,sdp.s, sdp.len);
-		offset += sdp.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-		memcpy(newbuf+offset, "\r\n--", 4);
-		offset+=4;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,ISUP_HEADER, strlen(ISUP_HEADER));
-		offset += strlen(ISUP_HEADER);
-
-		char * digits = calloc(1,destination->len+2);
-		memcpy(digits, destination->s, destination->len);
-		digits[destination->len] = '#';
+	char * digits = calloc(1,destination->len+2);
+	memcpy(digits, destination->s, destination->len);
+	digits[destination->len] = '#';
 
-		int res = isup_update_destination(digits, hops, int_nai, (unsigned char*)body.s, body.len, newbuf+offset, 512);
-
-		if(res == -1)
-		{
-			LM_DBG("error updating IAM\n");
-			return -1;
-		}
-
-		free(digits);
-		offset += res;
-
-		memcpy(newbuf+offset, "\r\n--", 4);
-		offset+=4;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "--\r\n", 4);
-		offset+=4;
-	}
-	else
+	int res = isup_update_destination(&mangle, digits, hops, int_nai, (unsigned char*)body.s, body.len);
+	free(digits);
+	if(res < 0)
 	{
-		// isup only body
-		char * digits = calloc(1,destination->len+2);
-		memcpy(digits, destination->s, destination->len);
-		digits[destination->len] = '#';
-
-		int res = isup_update_destination(digits, hops, int_nai, (unsigned char*)body.s, body.len, newbuf, 512);
-		free(digits);
-		offset = res;
-		if(res == -1)
-		{
-			LM_DBG("error updating IAM\n");
-			return -1;
-		}
+		LM_DBG("error updating IAM\n");
+		return -1;
 	}
 
 
 
-	str nb = {(char*)newbuf, offset};
-	replace_body(msg, &nb);
-
 	return 1;
 }
 
@@ -544,6 +383,7 @@ static int sipt_set_calling(struct sip_msg *msg, char *_origin, char *_nai, char
 	unsigned int int_nai = 0;
 	str2int(nai, &int_nai);
 	str * origin = (str*)_origin;
+	struct sdp_mangler mangle;
 
 	// update forwarded iam
 	str body;
@@ -554,11 +394,7 @@ static int sipt_set_calling(struct sip_msg *msg, char *_origin, char *_nai, char
 		LM_ERR("No ISUP Message Found");
 		return -1;
 	}
-	str sdp;
-	sdp.s = get_body_part(msg, TYPE_APPLICATION, SUBTYPE_SDP, &sdp.len);
-	
-	unsigned char newbuf[1024];
-	memset(newbuf, 0, 1024);
+
 	if (body.s==0) {
 		LM_ERR("failed to get the message body\n");
 		return -1;
@@ -576,88 +412,22 @@ static int sipt_set_calling(struct sip_msg *msg, char *_origin, char *_nai, char
 	}
 
 
+	mangle.msg = msg;
+	mangle.body_offset = (int)(body.s - msg->buf);
 
-	unsigned int offset = 0;
+	char * digits = calloc(1,origin->len+1);
+	memcpy(digits, origin->s, origin->len);
 
-	if(sdp.s != NULL)
+	int res = isup_update_calling(&mangle, digits, int_nai, pres, screen, (unsigned char*)body.s, body.len);
+	free(digits);
+	if(res < 0)
 	{
-		// we need to be clean, handle 2 cases
-		// one with sdp, one without sdp
-		str boundary = {0,0}; 
-		get_boundary_param(msg, &boundary);
-		memcpy(newbuf+offset, "--", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,SDP_HEADER, strlen(SDP_HEADER));
-		offset += strlen(SDP_HEADER);
-
-
-		memcpy(newbuf+offset,sdp.s, sdp.len);
-		offset += sdp.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-		memcpy(newbuf+offset, "\r\n--", 4);
-		offset+=4;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "\r\n", 2);
-		offset+=2;
-
-		memcpy(newbuf+offset,ISUP_HEADER, strlen(ISUP_HEADER));
-		offset += strlen(ISUP_HEADER);
-
-		char * digits = calloc(1,origin->len+1);
-		memcpy(digits, origin->s, origin->len);
-
-		int res = isup_update_calling(digits, int_nai, pres, screen, (unsigned char*)body.s, body.len, newbuf+offset, 512);
-
-		if(res == -1)
-		{
-			LM_DBG("error updating IAM\n");
-			return -1;
-		}
-
-		free(digits);
-		offset += res;
-
-		memcpy(newbuf+offset, "\r\n--", 4);
-		offset+=4;
-
-		memcpy(newbuf+offset,boundary.s, boundary.len);
-		offset += boundary.len;
-
-		memcpy(newbuf+offset, "--\r\n", 4);
-		offset+=4;
-	}
-	else
-	{
-		// isup only body
-		char * digits = calloc(1,origin->len+1);
-		memcpy(digits, origin->s, origin->len);
-
-		int res = isup_update_calling(digits, int_nai, pres, screen, (unsigned char*)body.s, body.len, newbuf, 512);
-		free(digits);
-		offset = res;
-		if(res == -1)
-		{
-			LM_DBG("error updating IAM\n");
-			return -1;
-		}
+		LM_DBG("error updating IAM\n");
+		return -1;
 	}
 
 
 
-	str nb = {(char*)newbuf, offset};
-	replace_body(msg, &nb);
 
 	return 1;
 }

modules/sipt/ss7.h

@@ -24,6 +24,8 @@
  * 
  */
 
+#include "sdp_mangle.h"
+
 #ifndef _SIPT_SS7_H_
 #define _SIPT_SS7_H_
 
@@ -179,8 +181,8 @@ int isup_get_calling_party_nai(unsigned char *buf, int len);
 int isup_get_called_party_nai(unsigned char *buf, int len);
 int isup_get_screening(unsigned char *buf, int len);
 int isup_get_presentation(unsigned char *buf, int len);
-int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf, int len, unsigned char * obuf, int olen);
-int isup_update_calling(char * origin, int nai, int presentation, int screening, unsigned char * buf, int len, unsigned char * obuf, int olen);
+int isup_update_destination(struct sdp_mangler * mangle, char * dest, int hops, int nai, unsigned char *buf, int len);
+int isup_update_calling(struct sdp_mangler * mangle, char * origin, int nai, int presentation, int screening, unsigned char * buf, int len);
 
 
 #endif

modules/sipt/ss7_parser.c

@@ -81,10 +81,11 @@ static void isup_put_number(unsigned char *dest, char *src, int *len, int *oddev
 		*oddeven = 0;
 		*len = numlen/2;
 	}
+	
 
 	while (i < numlen) {
 		if (!(i % 2))
-			dest[i/2] |= char2digit(src[i]) & 0xf;
+			dest[i/2] = char2digit(src[i]) & 0xf;
 		else
 		{
 			dest[i/2] |= (char2digit(src[i]) << 4) & 0xf0;
@@ -267,24 +268,20 @@ int isup_get_called_party_nai(unsigned char *buf, int len)
 	return message->called_party_number[1]&0x7F;
 }
 
-int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf, int len, unsigned char * obuf, int olen)
+int isup_update_destination(struct sdp_mangler * mangle, char * dest, int hops, int nai, unsigned char *buf, int len)
 {
-	int offset = 0, offset2 = 0;
-	int res;
-	char optparams;
-	unsigned char *param_pointer = NULL;
+	int offset = 0;
+	int res, res2;
+	struct isup_iam_fixed * orig_message = (struct isup_iam_fixed*)buf;
+	unsigned char tmp_buf[255];
 
 
 	// not an iam? do nothing
-	if(buf[0] != ISUP_IAM)
+	if(orig_message->type != ISUP_IAM)
 	{
-		int l = len > olen ? olen : len;
-		memcpy((void*)obuf, (void *)(buf), l);
-		return l;
+		return 1;
 	}
 
-	memset(obuf, 0, olen);
-
 	// bounds checking
 	if(hops > 31)
 	{
@@ -294,11 +291,8 @@ int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf,
 
 
 	/* Copy the fixed parms */
-	memcpy((void*)obuf, (void *)(buf), 6);
 	len -= 6;
-	olen -= 6;
 	offset += 6;
-	offset2 += 6;
 
 	if (len < 1)
 		return -1;
@@ -306,41 +300,29 @@ int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf,
 	/* IAM has one Fixed variable param, Called party number, we need to modify this */
 
 	// pointer to fixed part (2)
-	obuf[offset2++]=buf[offset++];
-
+	offset++;
 
 	//pointer to optional part (to update later)
-	param_pointer = (obuf+offset2);
-	optparams = buf[offset];
-	offset2++;
 	offset++;
 	len--;
-	olen--;
-
 
-	// add the new mandatory fixed header
-	res = encode_called_party(dest, buf+offset+1, nai, obuf+offset2+1, olen-1);
-	obuf[offset2] = (char)res;
 
-	offset2 += res+1;
-	olen -= res+1;
+	// modify the mandatory fixed header
+	res2 = encode_called_party(dest, buf+offset+1, nai, tmp_buf+1, 255-1);
+	tmp_buf[0] = (char)res2;
+	res = buf[offset]+1;
 	
-	res = buf[offset];
-	offset += res+1;
-	len -= res+1;
-
+	replace_body_segment(mangle, offset,res,tmp_buf, res2+1);
 
+	offset += res;
+	len -= res;
 	
 	if (len < 1 )
 		return -1;
 
 
-	
-	// set the pointer to the optional part
-	param_pointer[0] = (char)((obuf+offset2) - param_pointer);
-
 	/* Optional paramter parsing code */
-	if (optparams) {
+	if (orig_message->optional_pointer) {
 
 		bool has_hops = 0;
 		
@@ -352,14 +334,14 @@ int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf,
 			switch(optparm->type)
 			{
 				case ISUP_PARM_HOP_COUNTER:
-					obuf[offset2++] = ISUP_PARM_HOP_COUNTER;
-					obuf[offset2++] = 1;
-					obuf[offset2++] = ((optparm->data[0]&0x1F)-1)&0x1F;
+					tmp_buf[0] = ISUP_PARM_HOP_COUNTER;
+					tmp_buf[1] = 1;
+					tmp_buf[2] = ((optparm->data[0]&0x1F)-1)&0x1F;
+					replace_body_segment(mangle, offset, res, tmp_buf, 3);
 					has_hops = 1;
 					break;
 				default:
-					memcpy((void*)(obuf+offset2), (void *)(buf + offset), res);
-					offset2 += res;
+					break;
 			}
 
 			len -= res;
@@ -367,45 +349,34 @@ int isup_update_destination(char * dest, int hops, int nai, unsigned char *buf,
 		}
 
 		// add missing headers
-		if(!has_hops)
+		if(!has_hops && len >= 0)
 		{
-			obuf[offset2++] = ISUP_PARM_HOP_COUNTER;
-			obuf[offset2++] = 1;
-			obuf[offset2++] = hops & 0x1F;
+			tmp_buf[0] = ISUP_PARM_HOP_COUNTER;
+			tmp_buf[1] = 1;
+			tmp_buf[2] = hops & 0x1F;
 			has_hops = 1;
+			add_body_segment(mangle, offset,tmp_buf,3);
 		}
-
-		// nicely null terminate it
-		obuf[offset2++] = 0;
 	}
 
-	return offset2;
+	return offset;
 }
 
-int isup_update_calling(char * origin, int nai, int presentation, int screening, unsigned char * buf, int len, unsigned char * obuf, int olen)
+int isup_update_calling(struct sdp_mangler * mangle, char * origin, int nai, int presentation, int screening, unsigned char * buf, int len)
 {
-	int offset = 0, offset2 = 0;
+	int offset = 0;
 	int res;
-	struct isup_iam_fixed * message = (struct isup_iam_fixed*)obuf;
 	struct isup_iam_fixed * orig_message = (struct isup_iam_fixed*)buf;
 
 	// not an iam? do nothing
 	if(orig_message->type != ISUP_IAM)
 	{
-		int l = len > olen ? olen : len;
-		memcpy((void*)obuf, (void *)(buf), l);
-		return l;
+		return 1;
 	}
 
-	/* Message Type = 1 */
-	memset(obuf, 0, olen);
-
 	/* Copy the fixed parms */
-	memcpy((void*)obuf, (void *)(buf), offsetof(struct isup_iam_fixed, called_party_number));
 	len -= offsetof(struct isup_iam_fixed, called_party_number);
-	olen -= offsetof(struct isup_iam_fixed, called_party_number);
 	offset += offsetof(struct isup_iam_fixed, called_party_number);
-	offset2 += offsetof(struct isup_iam_fixed, called_party_number);
 
 	if (len < 1)
 		return -1;
@@ -416,22 +387,13 @@ int isup_update_calling(char * origin, int nai, int presentation, int screening,
 
 	// add the new mandatory fixed header
 	res = buf[offset];
-	memcpy((void*)(obuf+offset2), (void *)(buf+offset), res+1);
 	offset += res+1;
-	offset2 += res+1;
 	len -= res+1;
-	olen -= res+1;
-
-
 	
 	if (len < 1 )
 		return -1;
 
 
-	
-	// set the pointer to the optional part (there will be at least one optional header when we are done)
-	message->optional_pointer = (char)(offset2 - offsetof(struct isup_iam_fixed, optional_pointer));
-
 	/* Optional paramter parsing code */
 	if (orig_message->optional_pointer) {
 
@@ -440,44 +402,40 @@ int isup_update_calling(char * origin, int nai, int presentation, int screening,
 		while ((len > 0) && (buf[offset] != 0)) {
 			int res2 = 0;
 			struct isup_parm_opt *optparm = (struct isup_parm_opt *)(buf + offset);
+			unsigned char new_party[255];
 
 
 			res = optparm->len+2;
 			switch(optparm->type)
 			{
 				case ISUP_PARM_CALLING_PARTY_NUM:
-					obuf[offset2++] = ISUP_PARM_CALLING_PARTY_NUM;
-					res2 = encode_calling_party(origin, nai, presentation, screening, obuf+offset2+1, olen-1);
-					obuf[offset2] = (char)res2;
+					res2 = encode_calling_party(origin, nai, presentation, screening, &new_party[1], 255-1);
+					new_party[0] = (char)res2;
+					replace_body_segment(mangle, offset+1,(int)buf[offset+1],new_party, res2+1);
 
-					offset2 += res2+1;
-					olen -= res2+1;
 					has_calling = 1;
 					break;
 				default:
-					memcpy((void*)(obuf+offset2), (void *)(buf + offset), res);
-					offset2 += res;
+					break;
 			}
 
 			len -= res;
 			offset += res;
 		}
 
+
 		// add missing headers
-		if(!has_calling)
+		if(!has_calling && len >= 0)
 		{
-			obuf[offset2++] = ISUP_PARM_CALLING_PARTY_NUM;
-			res = encode_calling_party(origin, nai, presentation, screening, obuf+offset2+1, olen-1);
-			obuf[offset2] = (char)res;
+			unsigned char new_party[255];
+			new_party[0] = ISUP_PARM_CALLING_PARTY_NUM;
+			res = encode_calling_party(origin, nai, presentation, screening, new_party+2, 255-2);
+			new_party[1] = (char)res;
 
-			offset2 += res+1;
-			olen -= res+1;
-			has_calling = 1;
+			add_body_segment(mangle, offset,new_party, res+2);
 		}
 
-		// nicely null terminate it
-		obuf[offset2++] = 0;
 	}
 
-	return offset2;
+	return offset;
 }