frame *frame_alloc(sock *Conn)
{
frame *Frame = (frame *)s_malloc(sizeof(frame));
Frame->Pkt = pkt_alloc(Conn->buffsize);
Frame->state = FRAME_EMPTY;
return Frame;
}
static int psock_pkt_alloc(FAR struct socket *psock)
{
/* Allocate the packet socket connection structure and save in the new
* socket instance.
*/
FAR struct pkt_conn_s *conn = pkt_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
return -ENOMEM;
}
/* Set the reference count on the connection structure. This reference
* count will be incremented only if the socket is dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
conn->crefs = 1;
/* Save the pre-allocated connection in the socket structure */
psock->s_conn = conn;
return OK;
}
static void process_pkt(void* data) {
struct pkt_buff pkt;
struct ioq_header *ioq = data;
unsigned int size = ntohs(ioq->byte_length);
unsigned short dest = ntohs(ioq->src_port) ^ 2;
//print_pkt(data, size + sizeof(struct ioq_header));
log("%hd -> %hd (%d)\n", ntohs(ioq->src_port), dest, size);
// log("ioq_hdr: dst=%hx words=%hu src=%hu bytes=%hu\n", ntohs(ioq->dst_port), ntohs(ioq->word_length), ntohs(ioq->src_port), size);
pkt_fill(&pkt, data, size + sizeof(struct ioq_header));
struct pkt_buff reply;
struct ioq_header *dioq;
pkt_push_all(&pkt);
while (pkt_alloc(&reply, pkt.len) == 0) {
log("Failed to alloc\n");
}
memcpy32(reply.data+ sizeof(struct ioq_header),
pkt.data + sizeof(struct ioq_header),
reply.len - sizeof(struct ioq_header));
nf_pktin_free(data);
dioq = pkt_pull_type(&reply, struct ioq_header);
fill_ioq(dioq, dest, size);
nf_pktout_send(reply.data, reply.data + reply.total_size);
}
void pkt_to_nowhere(void)
{
sock *Conn = s_malloc(sizeof(sock));
pkt *Pkt = pkt_alloc(500);
uint32_t len = sizeof(Conn->local);
Conn->sock = s_socket(SOCK_DOMAIN, SOCK_TYPE, DEFAULT_PROTOCOL);
Conn->local.sin_family = SOCK_DOMAIN;
Conn->local.sin_addr.s_addr = htonl(INADDR_ANY); //???
Conn->local.sin_port = htons(DEFAULT_PORT);
s_bind(Conn->sock, (struct sockaddr *)&(Conn->local),
sizeof(Conn->local));
s_getsockname(Conn->sock, (struct sockaddr *)&(Conn->local),
(socklen_t *)&len);
create_pkt(Pkt, 0, 0, NULL, 0);
send_pkt(Pkt, Conn->sock, Conn->local);
shutdown(Conn->sock, SHUT_RDWR);
free(Conn);
}
void dissector_entry_point(uint8_t *packet, size_t len, int linktype, int mode)
{
struct protocol *proto_start, *proto_end;
struct pkt_buff *pkt = NULL;
if (mode == PRINT_NONE)
return;
pkt = pkt_alloc(packet, len);
switch (linktype) {
case LINKTYPE_EN10MB:
case ___constant_swab32(LINKTYPE_EN10MB):
proto_start = dissector_get_ethernet_entry_point();
proto_end = dissector_get_ethernet_exit_point();
break;
case LINKTYPE_IEEE802_11:
case ___constant_swab32(LINKTYPE_IEEE802_11):
proto_start = dissector_get_ieee80211_entry_point();
proto_end = dissector_get_ieee80211_exit_point();
break;
default:
panic("Linktype not supported!\n");
};
dissector_main(pkt, proto_start, proto_end);
switch (mode) {
case PRINT_HEX:
hex(pkt);
break;
case PRINT_ASCII:
ascii(pkt);
break;
case PRINT_HEX_ASCII:
hex_ascii(pkt);
break;
}
tprintf_flush();
pkt_free(pkt);
}
void dissector_entry_point(uint8_t *packet, struct frame_map *hdr, int linktype,
int mode, char **buffer_pkt,
uint8_t *switch_filter, uint8_t *stats)
{
struct protocol *proto_start = NULL;
struct protocol *proto_end = NULL;
struct pkt_buff *pkt = NULL;
if (mode == FNTTYPE_PRINT_NONE)
return;
pkt = pkt_alloc(packet, hdr->tp_h.tp_snaplen);
switch (linktype) {
case LINKTYPE_EN10MB:
proto_start = dissector_get_ethernet_entry_point();
proto_end = dissector_get_ethernet_exit_point();
break;
default:
panic("Linktype not supported!\n");
};
dissector_main(pkt, proto_start, proto_end, buffer_pkt, switch_filter,
stats);
switch (mode) {
case FNTTYPE_PRINT_HEX:
hex(pkt);
break;
case FNTTYPE_PRINT_ASCII:
ascii(pkt);
break;
case FNTTYPE_PRINT_HEX_ASCII:
hex_ascii(pkt);
break;
}
tprintf_flush();
pkt_free(pkt);
}
/*---------------------------------------------------------------------------*/
void icmp_pkt_rx (netif_t *netif, net_buf_t *net_buf)
{
icmp_hdr_t *icmp_pkt_in, *icmp_pkt_out ;
ipv4_hdr_t *ipv4_hdr_in, *ipv4_hdr_out ;
eth_hdr_t *eth_hdr_in ;
pkt_t *pkt ;
eth_hdr_in = (eth_hdr_t *) ((usys) net_buf->data) ;
ipv4_hdr_in = (ipv4_hdr_t *) (((usys) net_buf->data) + ETH_HDR_SIZE) ;
icmp_pkt_in = (icmp_hdr_t *) (((usys) net_buf->data) + ETH_HDR_SIZE +
IPV4_HDR_SIZE) ;
switch (icmp_pkt_in->type)
{
case MSG_TYPE_ECHO_REQUEST :
/* Alloc the frame */
if ((pkt = pkt_alloc (netif)) == NULL)
{
printf ("ERR: pkt_alloc() failed in handle_arp_pkt() !!\n") ;
return ;
}
/* Fill out the icmp header and data */
icmp_pkt_out = (icmp_hdr_t *) (((usys) pkt) +
ETH_HDR_SIZE +
IPV4_HDR_SIZE) ;
icmp_pkt_out->type = MSG_TYPE_ECHO_REPLY ;
icmp_pkt_out->code = 0 ;
icmp_pkt_out->id = icmp_pkt_in->id ;
icmp_pkt_out->seq = icmp_pkt_in->seq ;
memcpy (++icmp_pkt_out, ++icmp_pkt_in,
ntohs(ipv4_hdr_in->len) - IPV4_HDR_SIZE - ICMP_HDR_SIZE) ;
icmp_pkt_out -- ;
icmp_pkt_out->csum = ~crc16_calc ((u8 *) icmp_pkt_out,
ntohs(ipv4_hdr_in->len) -
IPV4_HDR_SIZE) ;
/* Initialize the ip header */
ipv4_hdr_out = (ipv4_hdr_t *) (((usys) pkt) + ETH_HDR_SIZE) ;
ipv4_hdr_init (netif, ipv4_hdr_out,
ntohs(ipv4_hdr_in->len),
IPV4_PROTOCOL_ICMP, ntohl(ipv4_hdr_in->src)) ;
/* Initialize the eth header */
eth_hdr_init (netif, pkt, eth_hdr_in->src_mac, ETH_TYPE_IPV4) ;
pkt_send (netif, pkt, ntohs(ipv4_hdr_in->len) +
ETH_HDR_SIZE) ;
pkt_free (netif, pkt) ;
break ;
default :
break ;
}
} /* End of function icmp_pkt_rx () */
int psock_socket(int domain, int type, int protocol, FAR struct socket *psock)
{
int err;
/* Only PF_INET, PF_INET6 or PF_PACKET domains supported */
if (
#if defined(CONFIG_NET_IPv6)
domain != PF_INET6
#else
domain != PF_INET
#endif
#if defined(CONFIG_NET_PKT)
&& domain != PF_PACKET
#endif
)
{
err = EAFNOSUPPORT;
goto errout;
}
/* Only SOCK_STREAM, SOCK_DGRAM and possible SOCK_RAW are supported */
if (
#if defined(CONFIG_NET_TCP)
(type == SOCK_STREAM && protocol != 0 && protocol != IPPROTO_TCP) ||
#endif
#if defined(CONFIG_NET_UDP)
(type == SOCK_DGRAM && protocol != 0 && protocol != IPPROTO_UDP) ||
#endif
(
#if defined(CONFIG_NET_TCP)
#if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_PKT)
type != SOCK_STREAM &&
#else
type != SOCK_STREAM
#endif
#endif
#if defined(CONFIG_NET_UDP)
#if defined(CONFIG_NET_PKT)
type != SOCK_DGRAM &&
#else
type != SOCK_DGRAM
#endif
#endif
#if defined(CONFIG_NET_PKT)
type != SOCK_RAW
#endif
)
)
{
err = EPROTONOSUPPORT;
goto errout;
}
/* Everything looks good. Initialize the socket structure */
/* Save the protocol type */
psock->s_type = type;
psock->s_conn = NULL;
#ifdef CONFIG_NET_TCP_WRITE_BUFFERS
psock->s_sndcb = NULL;
#endif
/* Allocate the appropriate connection structure. This reserves the
* the connection structure is is unallocated at this point. It will
* not actually be initialized until the socket is connected.
*/
err = ENOMEM; /* Assume failure to allocate connection instance */
switch (type)
{
#ifdef CONFIG_NET_PKT
case SOCK_RAW:
{
/* Allocate the packet socket connection structure and save
* in the new socket instance.
*/
FAR struct pkt_conn_s *conn = pkt_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout;
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
#ifdef CONFIG_NET_TCP
case SOCK_STREAM:
{
/* Allocate the TCP connection structure and save in the new
* socket instance.
*/
FAR struct tcp_conn_s *conn = tcp_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout; /* With err == ENFILE or ENOMEM */
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
#ifdef CONFIG_NET_UDP
case SOCK_DGRAM:
{
/* Allocate the UDP connection structure and save in the new
* socket instance.
*/
FAR struct udp_conn_s *conn = udp_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout; /* With err == ENFILE or ENOMEM */
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
default:
break;
}
return OK;
errout:
set_errno(err);
return ERROR;
}
