pb_stream *pbs_create(int size)
{
pb_stream *n;
void *mem;
n = alloc_thing(*n, "pbs_create");
mem=alloc_bytes(size, "pbs_memory");
init_pbs(n, mem, size, "tpm_pbs");
return n;
}
/* create output HDR as replica of input HDR */
void echo_hdr(struct msg_digest *md, bool enc, u_int8_t np)
{
struct isakmp_hdr r_hdr = md->hdr; /* mostly same as incoming header */
/* make sure we start with a clean buffer */
zero(reply_buffer);
init_pbs(&reply_stream, reply_buffer, sizeof(reply_buffer),
"reply packet");
r_hdr.isa_flags &= ~ISAKMP_FLAG_COMMIT; /* we won't ever turn on this bit */
if (enc)
r_hdr.isa_flags |= ISAKMP_FLAG_ENCRYPTION;
/* some day, we may have to set r_hdr.isa_version */
r_hdr.isa_np = np;
if (!out_struct(&r_hdr, &isakmp_hdr_desc, &reply_stream, &md->rbody))
impossible(); /* surely must have room and be well-formed */
}
bool
in_struct(void *struct_ptr, struct_desc *sd
, pb_stream *ins, pb_stream *obj_pbs)
{
err_t ugh = NULL;
u_int8_t *cur = ins->cur;
if (ins->roof - cur < (ptrdiff_t)sd->size) {
ugh = builddiag("not enough room in input packet for %s", sd->name);
} else {
u_int8_t *roof = cur + sd->size; /* may be changed by a length field */
u_int8_t *outp = struct_ptr;
bool immediate = FALSE;
field_desc *fp;
for (fp = sd->fields; ugh == NULL; fp++) {
size_t i = fp->size;
passert(ins->roof - cur >= (ptrdiff_t)i);
passert(cur - ins->cur <= (ptrdiff_t)(sd->size - i));
passert(outp - (cur - ins->cur) == struct_ptr);
#if 0
DBG(DBG_PARSING, DBG_log("%d %s"
, (int) (cur - ins->cur), fp->name == NULL? "" : fp->name));
#endif
switch (fp->field_type) {
case ft_mbz: /* must be zero */
for (; i != 0; i--) {
if (*cur++ != 0) {
ugh = builddiag("byte %d of %s must be zero, but is not"
, (int) (cur - ins->cur), sd->name);
break;
}
*outp++ = '\0'; /* probably redundant */
}
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_set: /* bits representing set */
{
u_int32_t n = 0;
for (; i != 0; i--)
n = (n << BITS_PER_BYTE) | *cur++;
switch (fp->field_type) {
case ft_len: /* length of this struct and any following crud */
case ft_lv: /* length/value field of attribute */
{
u_int32_t len = fp->field_type == ft_len? n
: immediate? sd->size : n + sd->size;
if (len < sd->size) {
ugh = builddiag("%s of %s is smaller than minimum"
, fp->name, sd->name);
} else if (pbs_left(ins) < len) {
ugh = builddiag("%s of %s is larger than can fit"
, fp->name, sd->name);
} else {
roof = ins->cur + len;
}
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;
}
i = fp->size;
switch (i) {
case 8/BITS_PER_BYTE:
*(u_int8_t *)outp = n;
break;
case 16/BITS_PER_BYTE:
*(u_int16_t *)outp = n;
break;
case 32/BITS_PER_BYTE:
*(u_int32_t *)outp = n;
break;
default:
impossible();
}
outp += i;
break;
}
case ft_raw: /* bytes to be left in network-order */
for (; i != 0; i--) {
*outp++ = *cur++;
}
break;
case ft_end: /* end of field list */
passert(cur == ins->cur + sd->size);
if (obj_pbs != NULL) {
init_pbs(obj_pbs, ins->cur, roof - ins->cur, sd->name);
obj_pbs->container = ins;
obj_pbs->desc = sd;
obj_pbs->cur = cur;
}
ins->cur = roof;
DBG(DBG_PARSING
, DBG_prefix_print_struct(ins, "parse ", struct_ptr, sd, TRUE));
return TRUE;
default:
impossible();
}
}
}
/* some failure got us here: report it */
loglog(RC_LOG_SERIOUS, ugh);
return FALSE;
}
void recv_pcap_packet_gen(u_char *user,
const struct pcap_pkthdr *h,
const u_char *bytes)
{
struct msg_digest *md;
u_int32_t *dlt;
struct iphdr *ip;
struct udphdr *udp;
u_char *ike;
const struct iface_port *ifp = &if1;
int packet_len;
err_t from_ugh;
union {
struct sockaddr sa;
struct sockaddr_in sa_in4;
struct sockaddr_in6 sa_in6;
} from;
md = alloc_md();
dlt = (u_int32_t *)bytes;
if (*dlt != PF_INET)
return;
ip = (struct iphdr *)(dlt + 1);
udp = (struct udphdr *)(dlt + ip->ihl + 1);
ike = (u_char *)(udp + 1);
from.sa_in4.sin_addr.s_addr = ip->saddr;
from.sa_in4.sin_port = udp->source;
md->iface = ifp;
packet_len = h->len - (ike - bytes);
happy(anyaddr(addrtypeof(&ifp->ip_addr), &md->sender));
from_ugh = initaddr((void *) &from.sa_in4.sin_addr,
sizeof(from.sa_in4.sin_addr),
AF_INET, &md->sender);
setportof(from.sa_in4.sin_port, &md->sender);
md->sender_port = ntohs(from.sa_in4.sin_port);
cur_from = &md->sender;
cur_from_port = md->sender_port;
/* Clone actual message contents
* and set up md->packet_pbs to describe it.
*/
init_pbs(&md->packet_pbs,
clone_bytes(ike, packet_len,
"message buffer in comm_handle()"),
packet_len, "packet");
DBG_log("*received %d bytes from %s:%u on %s (port=%d)",
(int) pbs_room(&md->packet_pbs),
ip_str(&md->sender), (unsigned) md->sender_port,
ifp->ip_dev->id_rname,
ifp->port);
DBG_dump("", md->packet_pbs.start, pbs_room(&md->packet_pbs));
process_packet(&md);
if (md != NULL)
release_md(md);
cur_state = NULL;
reset_cur_connection();
cur_from = NULL;
}
void send_v2_notification(struct state *p1st, u_int16_t type
, struct state *encst
, u_char *icookie
, u_char *rcookie
, chunk_t *n_data)
{
u_char buffer[1024];
pb_stream reply;
pb_stream rbody;
/* this function is not generic enough yet just enough for 6msg
* TBD accept HDR FLAGS as arg. default ISAKMP_FLAGS_R
* TBD when there is a child SA use that SPI in the notify paylod.
* TBD support encrypted notifications payloads.
* TBD accept Critical bit as an argument. default is set.
* TBD accept exchange type as an arg, default is ISAKMP_v2_SA_INIT
* do we need to send a notify with empty data?
* do we need to support more Protocol ID? more than PROTO_ISAKMP
*/
IPSEC_dbg("sending %snotification %s to %s:%u"
, encst ? "encrypted " : ""
, enum_name(&ikev2_notify_names, type)
, ip_str(&p1st->st_remoteaddr)
, p1st->st_remoteport);
if(n_data == NULL)
{
DBG(DBG_CONTROLMORE, DBG_log("don't send packet when notification data empty"));
return;
}
memset(buffer, 0, sizeof(buffer));
init_pbs(&reply, buffer, sizeof(buffer), "notification msg");
/* HDR out */
{
struct isakmp_hdr n_hdr ;
zero(&n_hdr); /* default to 0 */ /* AAA should we copy from MD? */
n_hdr.isa_version = IKEv2_MAJOR_VERSION << ISA_MAJ_SHIFT | IKEv2_MINOR_VERSION;
memcpy(n_hdr.isa_rcookie, rcookie, COOKIE_SIZE);
memcpy(n_hdr.isa_icookie, icookie, COOKIE_SIZE);
n_hdr.isa_xchg = ISAKMP_v2_SA_INIT;
n_hdr.isa_np = ISAKMP_NEXT_v2N;
n_hdr.isa_flags &= ~ISAKMP_FLAGS_I;
n_hdr.isa_flags |= ISAKMP_FLAGS_R;
if (!out_struct(&n_hdr, &isakmp_hdr_desc, &reply, &rbody))
{
IPSEC_dbg("error initializing hdr for notify message");
return;
}
}
chunk_t child_spi;
child_spi.ptr = NULL;
child_spi.len = 0;
/* build and add v2N payload to the packet */
ship_v2N (ISAKMP_NEXT_NONE, ISAKMP_PAYLOAD_CRITICAL,
PROTO_ISAKMP, &child_spi,type, n_data, &rbody);
close_message(&rbody);
close_output_pbs(&reply);
clonetochunk(p1st->st_tpacket, reply.start, pbs_offset(&reply)
, "notification packet");
ipsec_child_send_packet(p1st, "notification", TRUE);
}
