void pico_mock_destroy(struct pico_device *dev)
{
struct mock_device search = {
.dev = dev
};
struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search);
struct mock_frame*nxt;
if(!mock)
return;
nxt = mock->in_head;
while(nxt != NULL) {
mock->in_head = mock->in_head->next;
PICO_FREE(nxt);
nxt = mock->in_head;
}
nxt = mock->out_head;
while(nxt != NULL) {
mock->out_head = mock->out_head->next;
PICO_FREE(nxt);
nxt = mock->out_head;
}
pico_tree_delete(&mock_device_tree, mock);
}
static struct pico_dns_ns *pico_dns_client_add_ns(struct pico_ip4 *ns_addr)
{
struct pico_dns_ns *dns = NULL, *found = NULL, test = {{0}};
dns = pico_zalloc(sizeof(struct pico_dns_ns));
if (!dns) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dns->ns = *ns_addr;
found = pico_tree_insert(&NSTable, dns);
if (found) { /* nameserver already present */
pico_free(dns);
return found;
}
/* default NS found, remove it */
pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &test.ns.addr);
found = pico_tree_findKey(&NSTable, &test);
if (found && (found->ns.addr != ns_addr->addr))
pico_dns_client_del_ns(&found->ns);
return dns;
}
static struct pico_dns_key *pico_dns_client_idcheck(uint16_t id)
{
struct pico_dns_key test;
test.id = id;
return pico_tree_findKey(&DNSTable,&test);
}
static int pico_mock_poll(struct pico_device *dev, int loop_score)
{
struct mock_device search = {
.dev = dev
};
struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search);
struct mock_frame*nxt;
if(!mock)
return 0;
if (loop_score <= 0)
return 0;
while(mock->in_head != NULL && loop_score > 0)
{
pico_stack_recv(dev, mock->in_head->buffer, (uint32_t)mock->in_head->len);
loop_score--;
PICO_FREE(mock->in_head->buffer);
if(mock->in_tail == mock->in_head) {
PICO_FREE(mock->in_head);
mock->in_tail = mock->in_head = NULL;
return loop_score;
}
nxt = mock->in_head->next;
PICO_FREE(mock->in_head);
mock->in_head = nxt;
}
return loop_score;
}
static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr)
{
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_dhcp_server_setting test = {
0
};
if (pico_dhcp_server_find_negotiation(hdr->xid))
return NULL;
dhcpn = PICO_ZALLOC(sizeof(struct pico_dhcp_server_negotiation));
if (!dhcpn) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcpn->xid = hdr->xid;
dhcpn->state = PICO_DHCP_STATE_DISCOVER;
dhcpn->bcast = ((short_be(hdr->flags) & PICO_DHCP_FLAG_BROADCAST) != 0) ? (1) : (0);
memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH);
test.dev = dev;
dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test);
if (!dhcpn->dhcps) {
dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name);
PICO_FREE(dhcpn);
return NULL;
}
dhcp_negotiation_set_ciaddr(dhcpn);
pico_tree_insert(&DHCPNegotiations, dhcpn);
return dhcpn;
}
static struct pico_dhcp_client_cookie *pico_dhcp_client_add_cookie(uint32_t xid, struct pico_device *dev, void (*cb)(void *dhcpc, int code), uint32_t *uid)
{
struct pico_dhcp_client_cookie *dhcpc = NULL, *found = NULL, test = {
0
};
test.xid = xid;
found = pico_tree_findKey(&DHCPCookies, &test);
if (found) {
pico_err = PICO_ERR_EAGAIN;
return NULL;
}
dhcpc = pico_zalloc(sizeof(struct pico_dhcp_client_cookie));
if (!dhcpc) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcpc->state = DHCP_CLIENT_STATE_INIT;
dhcpc->status = DHCP_CLIENT_STATUS_INIT;
dhcpc->xid = xid;
dhcpc->uid = uid;
*(dhcpc->uid) = 0;
dhcpc->cb = cb;
dhcpc->dev = dev;
pico_tree_insert(&DHCPCookies, dhcpc);
return dhcpc;
}
static void pico_dns_client_retransmission(pico_time now, void *arg)
{
struct pico_dns_query *q = NULL;
struct pico_dns_query dummy;
IGNORE_PARAMETER(now);
if(!arg)
return;
/* search for the dns query and free used space */
dummy.id = *(uint16_t *)arg;
q = (struct pico_dns_query *)pico_tree_findKey(&DNSTable, &dummy);
PICO_FREE(arg);
/* dns query successful? */
if (!q) {
return;
}
if (q->retrans++ <= PICO_DNS_CLIENT_MAX_RETRANS) {
q->q_ns = pico_dns_client_next_ns(&q->q_ns.ns);
pico_dns_client_send(q);
} else {
pico_err = PICO_ERR_EIO;
q->callback(NULL, q->arg);
pico_dns_client_del_query(q->id);
}
}
static struct pico_dhcp_server_setting *pico_dhcp_server_add_setting(struct pico_dhcp_server_setting *setting)
{
uint16_t port = PICO_DHCPD_PORT;
struct pico_dhcp_server_setting *dhcps = NULL, *found = NULL, test = {0};
struct pico_ipv4_link *link = NULL;
link = pico_ipv4_link_get(&setting->server_ip);
if (!link) {
pico_err = PICO_ERR_EINVAL;
return NULL;
}
test.dev = setting->dev;
found = pico_tree_findKey(&DHCPSettings, &test);
if (found) {
pico_err = PICO_ERR_EINVAL;
return NULL;
}
dhcps = pico_zalloc(sizeof(struct pico_dhcp_server_setting));
if (!dhcps) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcps->lease_time = setting->lease_time;
dhcps->pool_start = setting->pool_start;
dhcps->pool_next = setting->pool_next;
dhcps->pool_end = setting->pool_end;
dhcps->dev = link->dev;
dhcps->server_ip = link->address;
dhcps->netmask = link->netmask;
/* default values if not provided */
if (!dhcps->pool_start)
dhcps->pool_start = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_START;
if (!dhcps->pool_end)
dhcps->pool_end = (dhcps->server_ip.addr & dhcps->netmask.addr) | DHCP_SERVER_POOL_END;
if (!dhcps->lease_time)
dhcps->lease_time = DHCP_SERVER_LEASE_TIME;
dhcps->pool_next = dhcps->pool_start;
dhcps->s = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &pico_dhcpd_wakeup);
if (!dhcps->s) {
dhcps_dbg("DHCP server ERROR: failure opening socket (%s)\n", strerror(pico_err));
pico_free(dhcps);
return NULL;
}
if (pico_socket_bind(dhcps->s, &dhcps->server_ip, &port) < 0) {
dhcps_dbg("DHCP server ERROR: failure binding socket (%s)\n", strerror(pico_err));
pico_free(dhcps);
return NULL;
}
pico_tree_insert(&DHCPSettings, dhcps);
return dhcps;
}
static struct pico_dhcp_server_negotiation *pico_dhcp_server_find_negotiation(uint32_t xid)
{
struct pico_dhcp_server_negotiation test = {0}, *found = NULL;
test.xid = xid;
found = pico_tree_findKey(&DHCPNegotiations, &test);
if (found)
return found;
else
return NULL;
}
static int ipfilter_apply_filter(struct pico_frame *f, struct filter_node *pkt)
{
struct filter_node *filter_frame = NULL;
filter_frame = pico_tree_findKey(&filter_tree, pkt);
if(filter_frame)
{
filter_frame->function_ptr(filter_frame, f);
return 1;
}
return 0;
}
static struct pico_dhcp_client_cookie *pico_dhcp_client_find_cookie(uint32_t xid)
{
struct pico_dhcp_client_cookie test = {
0
}, *found = NULL;
test.xid = xid;
found = pico_tree_findKey(&DHCPCookies, &test);
if (found)
return found;
else
return NULL;
}
static struct pico_dns_query *pico_dns_client_find_query(uint16_t id)
{
struct pico_dns_query test = {
0
}, *found = NULL;
test.id = id;
found = pico_tree_findKey(&DNSTable, &test);
if (found)
return found;
else
return NULL;
}
int pico_dhcp_server_destroy(struct pico_device *dev)
{
struct pico_dhcp_server_setting *found, test = { 0 };
test.dev = dev;
found = pico_tree_findKey(&DHCPSettings, &test);
if (!found) {
pico_err = PICO_ERR_ENOENT;
return -1;
}
pico_tree_delete(&DHCPSettings, found);
PICO_FREE(found);
return 0;
}
static int pico_dns_client_del_query(uint16_t id)
{
struct pico_dns_query test = {
0
}, *found = NULL;
test.id = id;
found = pico_tree_findKey(&DNSTable, &test);
if (!found)
return -1;
pico_free(found->query);
pico_socket_close(found->s);
pico_tree_delete(&DNSTable, found);
pico_free(found);
return 0;
}
static int pico_dhcp_client_del_cookie(uint32_t xid)
{
struct pico_dhcp_client_cookie test = {
0
}, *found = NULL;
test.xid = xid;
found = pico_tree_findKey(&DHCPCookies, &test);
if (!found)
return -1;
pico_socket_close(found->s);
pico_ipv4_link_del(found->dev, found->address);
pico_tree_delete(&DHCPCookies, found);
pico_free(found);
return 0;
}
static int pico_dns_client_del_ns(struct pico_ip4 *ns_addr)
{
struct pico_dns_ns test = {{0}}, *found = NULL;
test.ns = *ns_addr;
found = pico_tree_findKey(&NSTable, &test);
if (!found)
return -1;
pico_tree_delete(&NSTable, found);
pico_free(found);
/* no NS left, add default NS */
if (pico_tree_empty(&NSTable))
pico_dns_client_init();
return 0;
}
static void next_ping(uint32_t now, void *arg)
{
struct pico_icmp4_ping_cookie *newcookie, *cookie = (struct pico_icmp4_ping_cookie *)arg;
IGNORE_PARAMETER(now);
if(pico_tree_findKey(&Pings,cookie)){
if (cookie->seq < cookie->count) {
newcookie = pico_zalloc(sizeof(struct pico_icmp4_ping_cookie));
if (!newcookie)
return;
memcpy(newcookie, cookie, sizeof(struct pico_icmp4_ping_cookie));
newcookie->seq++;
pico_tree_insert(&Pings,newcookie);
send_ping(newcookie);
}
}
}
static struct pico_dhcp_server_setting *pico_dhcp_server_add_setting(struct pico_dhcp_server_setting *setting)
{
struct pico_dhcp_server_setting *dhcps = NULL, *found = NULL, test = {
0
};
struct pico_ipv4_link *link = NULL;
link = pico_ipv4_link_get(&setting->server_ip);
if (!link) {
pico_err = PICO_ERR_EINVAL;
return NULL;
}
test.dev = setting->dev;
found = pico_tree_findKey(&DHCPSettings, &test);
if (found) {
pico_err = PICO_ERR_EINVAL;
return NULL;
}
dhcps = PICO_ZALLOC(sizeof(struct pico_dhcp_server_setting));
if (!dhcps) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcps->lease_time = setting->lease_time;
dhcps->pool_start = setting->pool_start;
dhcps->pool_next = setting->pool_next;
dhcps->pool_end = setting->pool_end;
dhcps->dev = link->dev;
dhcps->server_ip = link->address;
dhcps->netmask = link->netmask;
/* default values if not provided */
dhcps_set_default_lease_time_if_not_provided(dhcps);
dhcps_set_default_pool_end_if_not_provided(dhcps);
dhcps_set_default_pool_start_if_not_provided(dhcps);
dhcps->pool_next = dhcps->pool_start;
return dhcps_try_open_socket(dhcps);
}
static void ping_timeout(uint32_t now, void *arg)
{
struct pico_icmp4_ping_cookie *cookie = (struct pico_icmp4_ping_cookie *)arg;
IGNORE_PARAMETER(now);
if(pico_tree_findKey(&Pings,cookie)){
if (cookie->err == PICO_PING_ERR_PENDING) {
struct pico_icmp4_stats stats;
stats.dst = cookie->dst;
stats.seq = cookie->seq;
stats.time = 0;
stats.size = cookie->size;
stats.err = PICO_PING_ERR_TIMEOUT;
dbg(" ---- Ping timeout!!!\n");
cookie->cb(&stats);
}
pico_tree_delete(&Pings,cookie);
pico_free(cookie);
}
}
static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr)
{
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_dhcp_server_setting test = {0};
struct pico_ip4 *ciaddr = NULL;
if (pico_dhcp_server_find_negotiation(hdr->xid))
return NULL;
dhcpn = pico_zalloc(sizeof(struct pico_dhcp_server_negotiation));
if (!dhcpn) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcpn->xid = hdr->xid;
dhcpn->state = PICO_DHCP_STATE_DISCOVER;
memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH);
test.dev = dev;
dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test);
if (!dhcpn->dhcps) {
dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name);
pico_free(dhcpn);
return NULL;
}
ciaddr = pico_arp_reverse_lookup(&dhcpn->hwaddr);
if (!ciaddr) {
dhcpn->ciaddr.addr = dhcpn->dhcps->pool_next;
dhcpn->dhcps->pool_next = long_be(long_be(dhcpn->dhcps->pool_next) + 1);
pico_arp_create_entry(dhcpn->hwaddr.addr, dhcpn->ciaddr, dhcpn->dhcps->dev);
} else {
dhcpn->ciaddr = *ciaddr;
}
pico_tree_insert(&DHCPNegotiations, dhcpn);
return dhcpn;
}
static void ping_recv_reply(struct pico_frame *f)
{
struct pico_icmp4_ping_cookie test, *cookie;
struct pico_icmp4_hdr *hdr = (struct pico_icmp4_hdr *) f->transport_hdr;
test.id = short_be(hdr->hun.ih_idseq.idseq_id );
test.seq = short_be(hdr->hun.ih_idseq.idseq_seq);
cookie = pico_tree_findKey(&Pings, &test);
if (cookie) {
struct pico_icmp4_stats stats;
cookie->err = PICO_PING_ERR_REPLIED;
stats.dst = cookie->dst;
stats.seq = cookie->seq;
stats.size = cookie->size;
stats.time = pico_tick - cookie->timestamp;
stats.err = cookie->err;
stats.ttl = ((struct pico_ipv4_hdr *)f->net_hdr)->ttl;
if(cookie->cb != NULL)
cookie->cb(&stats);
} else {
dbg("Reply for seq=%d, not found.\n", test.seq);
}
}
static int pico_mock_send(struct pico_device *dev, void *buf, int len)
{
struct mock_device search = {
.dev = dev
};
struct mock_device*mock = pico_tree_findKey(&mock_device_tree, &search);
struct mock_frame*frame;
if(!mock)
return 0;
if (len > MOCK_MTU)
return 0;
frame = PICO_ZALLOC(sizeof(struct mock_frame));
if(!frame) {
return 0;
}
if(mock->out_head == NULL)
mock->out_head = frame;
else
mock->out_tail->next = frame;
mock->out_tail = frame;
mock->out_tail->buffer = PICO_ZALLOC((uint32_t)len);
if(!mock->out_tail->buffer)
return 0;
memcpy(mock->out_tail->buffer, buf, (uint32_t)len);
mock->out_tail->len = len;
return len;
}
static void pico_dns_client_callback(uint16_t ev, struct pico_socket *s)
{
char *q_qname, *q_suf, *a_hdr, *a_qname, *a_suf, *a_rdata;
struct dns_message_hdr *hdr;
struct dns_query_suffix query_suf;
struct dns_answer_suffix answer_suf;
struct pico_dns_key test, *key;
char *answer;
char dns_answer[PICO_DNS_MAX_RESPONSE_LEN] = {0};
uint8_t valid_suffix = 0;
uint16_t compression = 0;
int i = 0, r = 0;
if (ev & PICO_SOCK_EV_RD) {
r = pico_socket_read(s, dns_answer, PICO_DNS_MAX_RESPONSE_LEN);
pico_socket_close(s);
if (r == PICO_DNS_MAX_RESPONSE_LEN || r < (int)sizeof(struct dns_message_hdr)) {
dns_dbg("DNS ERROR: received incorrect number(%d) of bytes\n", r);
return;
}
/* Check header validity */
a_hdr = dns_answer;
hdr = (struct dns_message_hdr *) a_hdr;
pico_dns_client_hdr_ntoh(hdr);
if (GET_FLAG_QR(hdr) != PICO_DNS_QR_RESPONSE || GET_FLAG_OPCODE(hdr) != PICO_DNS_OPCODE_QUERY
|| GET_FLAG_TC(hdr) == PICO_DNS_TC_IS_TRUNCATED || GET_FLAG_RCODE(hdr) != PICO_DNS_RCODE_NO_ERROR) {
dns_dbg("DNS ERROR: OPCODE %d | TC %d | RCODE %d\n", GET_FLAG_OPCODE(hdr), GET_FLAG_TC(hdr), GET_FLAG_RCODE(hdr));
return;
}
if (hdr->ancount < 1 || r < (int)(sizeof(struct dns_message_hdr) + hdr->qdcount * sizeof(struct dns_query_suffix)
+ hdr->ancount * sizeof(struct dns_answer_suffix))) {
dns_dbg("DNS ERROR: ancount < 1 OR received number(%d) of bytes too low\n", r);
return;
}
/* Find DNS key */
test.id = hdr->id;
key = pico_tree_findKey(&DNSTable,&test);
if (!key) {
dns_dbg("DNS WARNING: key with id %u not found\n", hdr->id);
return;
}
key->retrans = 0;
/* Check query suffix validity */
q_qname = a_hdr + sizeof(struct dns_message_hdr);
q_suf = pico_dns_client_seek(q_qname);
query_suf = *(struct dns_query_suffix *) q_suf;
if (short_be(query_suf.qtype) != key->qtype || short_be(query_suf.qclass) != key->qclass) {
dns_dbg("DNS ERROR: received qtype (%u) or qclass (%u) incorrect\n", short_be(query_suf.qtype), short_be(query_suf.qclass));
return;
}
/* Seek answer suffix */
a_qname = q_suf + sizeof(struct dns_query_suffix);
a_suf = a_qname;
while(i++ < hdr->ancount) {
uint16_t comp_h = short_from(a_suf);
compression = short_be(comp_h);
switch (compression >> 14)
{
case PICO_DNS_POINTER:
while (compression >> 14 == PICO_DNS_POINTER) {
dns_dbg("DNS: pointer\n");
a_suf += sizeof(uint16_t);
comp_h = short_from(a_suf);
compression = short_be(comp_h);
}
break;
case PICO_DNS_LABEL:
dns_dbg("DNS: label\n");
a_suf = pico_dns_client_seek(a_qname);
break;
default:
dns_dbg("DNS ERROR: incorrect compression (%u) value\n", compression);
return;
}
/* Check answer suffix validity */
answer_suf = *(struct dns_answer_suffix *)a_suf;
if (short_be(answer_suf.qtype) != key->qtype || short_be(answer_suf.qclass) != key->qclass) {
dns_dbg("DNS WARNING: received qtype (%u) or qclass (%u) incorrect\n", short_be(answer_suf.qtype), short_be(answer_suf.qclass));
a_suf = a_suf + sizeof(struct dns_answer_suffix) + short_be(answer_suf.rdlength);
continue;
}
if (short_be(answer_suf.ttl) > PICO_DNS_MAX_TTL) {
dns_dbg("DNS WARNING: received TTL (%u) > MAX (%u)\n", short_be(answer_suf.ttl), PICO_DNS_MAX_TTL);
a_suf = a_suf + sizeof(struct dns_answer_suffix) + short_be(answer_suf.rdlength);
continue;
}
valid_suffix = 1;
break;
}
if (!valid_suffix) {
dns_dbg("DNS ERROR: invalid dns answer suffix\n");
return;
}
a_rdata = a_suf + sizeof(struct dns_answer_suffix);
if (key->qtype == PICO_DNS_TYPE_A) {
uint32_t ip_h = long_from(a_rdata);
dns_dbg("DNS: length %u | ip %08X\n", short_be(answer_suf.rdlength), long_be(ip_h));
answer = pico_zalloc(16);
pico_ipv4_to_string(answer, ip_h);
key->callback(answer, key->arg);
} else if (key->qtype == PICO_DNS_TYPE_PTR) {
pico_dns_client_reverse_label((char *) a_rdata);
dns_dbg("DNS: length %u | name %s\n", short_be(answer_suf.rdlength), (char *)a_rdata + 1);
answer = pico_zalloc(answer_suf.rdlength - 1);
memcpy(answer, (char *)a_rdata + 1, short_be(answer_suf.rdlength) - 1);
key->callback(answer, key->arg);
} else {
dns_dbg("DNS ERROR: incorrect qtype (%u)\n", key->qtype);
return;
}
}
int pico_dns_client_nameserver(struct pico_ip4 *ns, uint8_t flag)
{
struct pico_dns_ns test, *key = NULL;
if (!ns) {
pico_err = PICO_ERR_EINVAL;
return -1;
}
switch (flag)
{
case PICO_DNS_NS_ADD:
key = pico_zalloc(sizeof(struct pico_dns_ns));
if (!key) {
pico_err = PICO_ERR_ENOMEM;
return -1;
}
key->ns = *ns;
if(pico_tree_insert(&NSTable,key)){
dns_dbg("DNS WARNING: nameserver %08X already added\n",ns->addr);
pico_err = PICO_ERR_EINVAL;
pico_free(key);
return -1; /* Element key already exists */
}
dns_dbg("DNS: nameserver %08X added\n", ns->addr);
/* If default NS found, remove it */
pico_string_to_ipv4(PICO_DNS_NS_GOOGLE, &test.ns.addr);
if (ns->addr != test.ns.addr) {
key = pico_tree_findKey(&NSTable,&test);
if (key) {
if(pico_tree_delete(&NSTable,key)) {
dns_dbg("DNS: default nameserver %08X removed\n", test.ns.addr);
pico_free(key);
} else {
pico_err = PICO_ERR_EAGAIN;
return -1;
}
}
}
break;
case PICO_DNS_NS_DEL:
test.ns = *ns;
key = pico_tree_findKey(&NSTable,&test);
if (!key) {
dns_dbg("DNS WARNING: nameserver %08X not found\n", ns->addr);
pico_err = PICO_ERR_EINVAL;
return -1;
}
/* RB_REMOVE returns pointer to removed element, NULL to indicate error */
if(pico_tree_delete(&NSTable,key)) {
dns_dbg("DNS: nameserver %08X removed\n",key->ns.addr);
pico_free(key);
} else {
pico_err = PICO_ERR_EAGAIN;
return -1;
}
/* If no NS left, add default NS */
if(pico_tree_first(&NSTable) == NULL){
dns_dbg("DNS: add default nameserver\n");
return pico_dns_client_init();
}
break;
default:
pico_err = PICO_ERR_EINVAL;
return -1;
}
return 0;
}
