packet.c

This a good VPN source

	if (bytes == NULL)
	{
	    DBG(DBG_PARSING
		, DBG_log("skipping %u raw bytes of %s (%s)"
		    , (unsigned) len, ins->name, name);
		  DBG_dump(name, ins->cur, len));
	}
	else
	{
	    memcpy(bytes, ins->cur, len);
	    DBG(DBG_PARSING
		, DBG_log("parsing %u raw bytes of %s into %s"
		    , (unsigned) len, ins->name, name);
		  DBG_dump(name, bytes, len));
	}
	ins->cur += len;
	return TRUE;
    }
}

/* "emit" a host struct into a network packet.
 *
 * This code assumes that the network and host structure
 * members have the same alignment and size!  This requires
 * that all padding be explicit.
 *
 * If obj_pbs is non-NULL, its pbs describes a new output stream set up
 * to contain the object.  The cursor will be left at the variable part.
 * This new stream must subsequently be finalized by close_output_pbs().
 *
 * The value of any field of type ft_len is computed, not taken
 * from the input struct.  The length is actually filled in when
 * the object's output stream is finalized.  If obj_pbs is NULL,
 * finalization is done by out_struct before it returns.
 *
 * This routine returns TRUE iff it succeeds.
 */

bool
out_struct(const void *struct_ptr, struct_desc *sd
	   , pb_stream *outs, pb_stream *obj_pbs)
{
    err_t ugh = NULL;
    const u_int8_t *inp = struct_ptr;
    u_int8_t *cur = outs->cur;

    DBG(DBG_EMITTING
	, DBG_prefix_print_struct(outs, "emit ", struct_ptr, sd, obj_pbs==NULL));

    if (outs->roof - cur < (ptrdiff_t)sd->size)
    {
	ugh = builddiag("not enough room left in output packet to place %s"
	    , sd->name);
    }
    else
    {
	bool immediate = FALSE;
	pb_stream obj;
	field_desc *fp;

	obj.lenfld = NULL;  /* until a length field is discovered */
	obj.lenfld_desc = NULL;

	for (fp = sd->fields; ugh == NULL; fp++)
	{
	    size_t i = fp->size;

	    passert(outs->roof - cur >= (ptrdiff_t)i);
	    passert(cur - outs->cur <= (ptrdiff_t)(sd->size - i));
	    passert(inp - (cur - outs->cur) == struct_ptr);

#if 0
	    DBG(DBG_EMITTING, DBG_log("%d %s"
		, (int) (cur - outs->cur), fp->name == NULL? "" : fp->name);
#endif
	    switch (fp->field_type)
	    {
	    case ft_mbz:	/* must be zero */
		inp += i;
		for (; i != 0; i--)
		    *cur++ = '\0';
		break;
	    case ft_nat:	/* natural number (may be 0) */
	    case ft_len:	/* length of this struct and any following crud */
	    case ft_lv:		/* length/value field of attribute */
	    case ft_enum:	/* value from an enumeration */
	    case ft_loose_enum:	/* value from an enumeration with only some names known */
	    case ft_af_enum:	/* Attribute Format + value from an enumeration */
	    case ft_af_loose_enum: /* Attribute Format + value from an enumeration */
	    case ft_set:	/* bits representing set */
	    {
		u_int32_t n = 0;

		switch (i)
		{
		case 8/BITS_PER_BYTE:
		    n = *(const u_int8_t *)inp;
		    break;
		case 16/BITS_PER_BYTE:
		    n = *(const u_int16_t *)inp;
		    break;
		case 32/BITS_PER_BYTE:
		    n = *(const u_int32_t *)inp;
		    break;
		default:
		    bad_case(i);
		}

		switch (fp->field_type)
		{
		case ft_len:	/* length of this struct and any following crud */
		case ft_lv:	/* length/value field of attribute */
		    if (immediate)
			break;	/* not a length */
		    /* We can't check the length because it will likely
		     * be filled in after variable part is supplied.
		     * We do record where this is so that it can be
		     * filled in by a subsequent close_output_pbs().
		     */
		    passert(obj.lenfld == NULL);	/* only one ft_len allowed */
		    obj.lenfld = cur;
		    obj.lenfld_desc = fp;
		    break;
		case ft_af_loose_enum: /* Attribute Format + value from an enumeration */
		    if ((n & ISAKMP_ATTR_AF_MASK) == ISAKMP_ATTR_AF_TV)
			immediate = TRUE;
		    break;

		case ft_af_enum:	/* Attribute Format + value from an enumeration */
		    if ((n & ISAKMP_ATTR_AF_MASK) == ISAKMP_ATTR_AF_TV)
			immediate = TRUE;
		    /* FALL THROUGH */
		case ft_enum:	/* value from an enumeration */
		    if (enum_name(fp->desc, n) == NULL)
		    {
			ugh = builddiag("%s of %s has an unknown value: %lu"
			    , fp->name, sd->name, (unsigned long)n);
		    }
		    /* FALL THROUGH */
		case ft_loose_enum:	/* value from an enumeration with only some names known */
		    break;
		case ft_set:	/* bits representing set */
		    if (!testset(fp->desc, n))
		    {
			ugh = builddiag("bitset %s of %s has unknown member(s): %s"
			    , fp->name, sd->name, bitnamesof(fp->desc, n));
		    }
		    break;
		default:
		    break;
		}

		while (i-- != 0)
		{
		    cur[i] = (u_int8_t)n;
		    n >>= BITS_PER_BYTE;
		}
		inp += fp->size;
		cur += fp->size;
		break;
	    }
	    case ft_raw:	/* bytes to be left in network-order */
		for (; i != 0; i--)
		    *cur++ = *inp++;
		break;
	    case ft_end:	/* end of field list */
		passert(cur == outs->cur + sd->size);

		obj.container = outs;
		obj.desc = sd;
		obj.name = sd->name;
		obj.start = outs->cur;
		obj.cur = cur;
		obj.roof = outs->roof;	/* limit of possible */
		/* obj.lenfld and obj.lenfld_desc already set */

		if (obj_pbs == NULL)
		{
		    close_output_pbs(&obj); /* fill in length field, if any */
		}
		else
		{
		    /* We set outs->cur to outs->roof so that
		     * any attempt to output something into outs
		     * before obj is closed will trigger an error.
		     */
		    outs->cur = outs->roof;

		    *obj_pbs = obj;
		}
		return TRUE;

	    default:
		bad_case(fp->field_type);
	    }
	}
    }

    /* some failure got us here: report it */
    loglog(RC_LOG_SERIOUS, ugh);	/* ??? serious, but errno not relevant */
    return FALSE;
}

bool
out_modify_previous_np(u_int8_t np, pb_stream *outs)
{
	size_t len = (outs->cur - outs->start), offset;
	if (len < sizeof(struct isakmp_hdr)) {
		return FALSE;
	}
	else if (len == sizeof(struct isakmp_hdr)) {
		struct isakmp_hdr *hdr = (struct isakmp_hdr *)outs->start;
		hdr->isa_np = np;
		return TRUE;
	}
	else {
		struct isakmp_generic *hdr;
		for (offset = sizeof(struct isakmp_hdr); offset < len ;
			offset += ntohs(hdr->isag_length)) {
			if ((len - offset) < sizeof(struct isakmp_generic))
				return FALSE;
			hdr = (struct isakmp_generic *)(outs->start+offset);
			if ((len - offset) < ntohs(hdr->isag_length))
				return FALSE;
			if ((len - offset) == ntohs(hdr->isag_length)) {
				hdr->isag_np = np;
				return TRUE;
			}
		}
	}
	return FALSE;
}

bool
out_generic(u_int8_t np, struct_desc *sd
, pb_stream *outs, pb_stream *obj_pbs)
{
    struct isakmp_generic gen;

    passert(sd->fields == isakmp_generic_desc.fields);
    gen.isag_np = np;
    return out_struct(&gen, sd, outs, obj_pbs);
}

bool
out_generic_raw(u_int8_t np, struct_desc *sd
, pb_stream *outs, const void *bytes, size_t len, const char *name)
{
    pb_stream pbs;

    if (!out_generic(np, sd, outs, &pbs)
    || !out_raw(bytes, len, &pbs, name))
	return FALSE;
    close_output_pbs(&pbs);
    return TRUE;
}

bool
out_raw(const void *bytes, size_t len, pb_stream *outs, const char *name)
{
    if (pbs_left(outs) < len)
    {
	loglog(RC_LOG_SERIOUS, "not enough room left to place %lu bytes of %s in %s"
	    , (unsigned long) len, name, outs->name);
	return FALSE;
    }
    else
    {
	DBG(DBG_EMITTING
	    , DBG_log("emitting %u raw bytes of %s into %s"
		, (unsigned) len, name, outs->name);
	      DBG_dump(name, bytes, len));
	memcpy(outs->cur, bytes, len);
	outs->cur += len;
	return TRUE;
    }
}

bool
out_zero(size_t len, pb_stream *outs, const char *name)
{
    if (pbs_left(outs) < len)
    {
	loglog(RC_LOG_SERIOUS, "not enough room left to place %s in %s", name, outs->name);
	return FALSE;
    }
    else
    {
	DBG(DBG_EMITTING, DBG_log("emitting %u zero bytes of %s into %s"
	    , (unsigned) len, name, outs->name));
	memset(outs->cur, 0x00, len);
	outs->cur += len;
	return TRUE;
    }
}

/* Record current length.
 * Note: currently, this may be repeated any number of times;
 * the last one wins.
 */
void
close_output_pbs(pb_stream *pbs)
{
    if (pbs->lenfld != NULL)
    {
	u_int32_t len = pbs_offset(pbs);
	int i = pbs->lenfld_desc->size;

	if (pbs->lenfld_desc->field_type == ft_lv)
	    len -= sizeof(struct isakmp_attribute);
	DBG(DBG_EMITTING, DBG_log("emitting length of %s: %lu"
	    , pbs->name, (unsigned long) len));
	while (i-- != 0)
	{
	    pbs->lenfld[i] = (u_int8_t)len;
	    len >>= BITS_PER_BYTE;
	}
    }
    if (pbs->container != NULL)
	pbs->container->cur = pbs->cur;	/* pass space utilization up */
}