static void rtcfg_client_recv_stage_2_frag(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_2_cfg_frag *stage_2_frag;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
size_t data_len;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_2_cfg_frag)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_2_cfg_frag frame\n");
kfree_rtskb(rtskb);
return;
}
stage_2_frag = (struct rtcfg_frm_stage_2_cfg_frag *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg_frag));
data_len = MIN(rtcfg_dev->spec.clt.cfg_len - rtcfg_dev->spec.clt.cfg_offs,
rtskb->len);
if ((rtcfg_dev->flags & RTCFG_FLAG_STAGE_2_DATA) == 0) {
RTCFG_DEBUG(1, "RTcfg: unexpected stage 2 fragment, we did not "
"request any data!\n");
} else if (rtcfg_dev->spec.clt.cfg_offs !=
ntohl(stage_2_frag->frag_offs)) {
RTCFG_DEBUG(1, "RTcfg: unexpected stage 2 fragment (expected: %d, "
"received: %d)\n", rtcfg_dev->spec.clt.cfg_offs,
ntohl(stage_2_frag->frag_offs));
rtcfg_send_ack(ifindex);
rtcfg_dev->spec.clt.packet_counter = 0;
} else {
rtcfg_client_queue_frag(ifindex, rtskb, data_len);
rtskb = NULL;
}
rtos_res_unlock(&rtcfg_dev->dev_lock);
if (rtskb != NULL)
kfree_rtskb(rtskb);
}
int rt_ip_local_deliver_finish(struct rtskb *skb)
{
int ihl = skb->nh.iph->ihl*4;
int hash = rt_inet_hashkey(skb->nh.iph->protocol);
int ret = 0;
struct rtinet_protocol *ipprot = rt_inet_protocols[hash];
__rtskb_pull(skb, ihl);
/* Point into the IP datagram, just past the header. */
skb->h.raw = skb->data;
if ( ipprot )
ret = ipprot->handler(skb);
else {
rt_printk("RTnet: no protocol found\n");
kfree_rtskb(skb);
}
return ret;
}
static void rtcfg_client_recv_stage_2_cfg(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_2_cfg *stage_2_cfg;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
size_t data_len;
int ret;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_2_cfg)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_2_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_2_cfg = (struct rtcfg_frm_stage_2_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg));
if (stage_2_cfg->heartbeat_period) {
ret = rtos_task_init_periodic(&rtcfg_dev->timer_task, rtcfg_timer,
(void *)ifindex, RTOS_LOWEST_RT_PRIORITY,
((nanosecs_t)ntohs(stage_2_cfg->heartbeat_period)) * 1000000);
if (ret < 0)
/*ERRMSG*/rtos_print("RTcfg: unable to create timer task\n");
else
rtcfg_dev->flags |= FLAG_TIMER_STARTED;
}
/* add server to station list */
if (rtcfg_add_to_station_list(rtcfg_dev,
rtskb->mac.ethernet->h_source, stage_2_cfg->flags) < 0) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: unable to process stage_2_cfg frage\n");
kfree_rtskb(rtskb);
return;
}
rtcfg_dev->other_stations = ntohl(stage_2_cfg->stations);
rtcfg_dev->spec.clt.cfg_len = ntohl(stage_2_cfg->cfg_len);
data_len = MIN(rtcfg_dev->spec.clt.cfg_len, rtskb->len);
if (((rtcfg_dev->flags & RTCFG_FLAG_STAGE_2_DATA) != 0) &&
(data_len > 0)) {
rtcfg_client_queue_frag(ifindex, rtskb, data_len);
rtskb = NULL;
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations)
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_KNOWN);
} else {
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations) {
rtcfg_complete_cmd(ifindex, RTCFG_CMD_ANNOUNCE, 0);
rtcfg_next_main_state(ifindex,
((rtcfg_dev->flags & RTCFG_FLAG_READY) != 0) ?
RTCFG_MAIN_CLIENT_READY : RTCFG_MAIN_CLIENT_2);
} else
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_FRAMES);
rtcfg_send_ack(ifindex);
}
rtos_res_unlock(&rtcfg_dev->dev_lock);
if (rtskb != NULL)
kfree_rtskb(rtskb);
}
static void rtcfg_client_recv_stage_1(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_1_cfg *stage_1_cfg;
struct rt_proc_call *call;
struct rtcfg_cmd *cmd_event;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
u8 addr_type;
int ret;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_1_cfg));
addr_type = stage_1_cfg->addr_type;
switch (stage_1_cfg->addr_type) {
#ifdef CONFIG_RTNET_RTIPV4
case RTCFG_ADDR_IP: {
struct rtnet_device *rtdev, *tmp;
u32 daddr, saddr, mask, bcast;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg) +
2*RTCFG_ADDRSIZE_IP) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg "
"frame\n");
kfree_rtskb(rtskb);
break;
}
rtdev = rtskb->rtdev;
daddr = *(u32*)stage_1_cfg->client_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
saddr = *(u32*)stage_1_cfg->server_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
__rtskb_pull(rtskb, 2*RTCFG_ADDRSIZE_IP);
/* Broadcast: IP is used to address client */
if (rtskb->pkt_type == PACKET_BROADCAST) {
/* directed to us? */
if (daddr != rtdev->local_ip) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
kfree_rtskb(rtskb);
return;
}
/* Unicast: IP address is assigned by the server */
} else {
/* default netmask */
if (ntohl(daddr) <= 0x7FFFFFFF) /* 127.255.255.255 */
mask = 0x000000FF; /* 255.0.0.0 */
else if (ntohl(daddr) <= 0xBFFFFFFF) /* 191.255.255.255 */
mask = 0x0000FFFF; /* 255.255.0.0 */
else
mask = 0x00FFFFFF; /* 255.255.255.0 */
bcast = daddr | (~mask);
rt_ip_route_del_all(rtdev); /* cleanup routing table */
rtdev->local_ip = daddr;
rtdev->broadcast_ip = bcast;
if ((tmp = rtdev_get_loopback()) != NULL) {
rt_ip_route_add_host(daddr, tmp->dev_addr, tmp);
rtdev_dereference(tmp);
}
if (rtdev->flags & IFF_BROADCAST)
rt_ip_route_add_host(bcast, rtdev->broadcast, rtdev);
}
/* update routing table */
rt_ip_route_add_host(saddr, rtskb->mac.ethernet->h_source, rtdev);
rtcfg_dev->spec.clt.srv_addr.ip_addr = saddr;
break;
}
#endif /* CONFIG_RTNET_RTIPV4 */
case RTCFG_ADDR_MAC:
/* nothing to do */
break;
default:
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: unknown addr_type %d in %s()\n",
stage_1_cfg->addr_type, __FUNCTION__);
kfree_rtskb(rtskb);
return;
}
rtcfg_dev->spec.clt.addr_type = addr_type;
/* Ethernet-specific */
memcpy(rtcfg_dev->spec.clt.srv_mac_addr,
rtskb->mac.ethernet->h_source, ETH_ALEN);
rtcfg_dev->burstrate = stage_1_cfg->burstrate;
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_1);
rtos_res_unlock(&rtcfg_dev->dev_lock);
while (1) {
call = rtcfg_dequeue_blocking_call(ifindex);
if (call == NULL)
break;
cmd_event = rtpc_get_priv(call, struct rtcfg_cmd);
if (cmd_event->event_id == RTCFG_CMD_CLIENT) {
ret = 0;
/* note: only the first pending call gets data */
if ((rtskb != NULL) &&
(cmd_event->args.client.buffer_size > 0)) {
ret = ntohs(stage_1_cfg->cfg_len);
cmd_event->args.client.rtskb = rtskb;
rtskb = NULL;
}
} else
ret = -EINVAL;
rtpc_complete_call(call, ret);
}
if (rtskb)
kfree_rtskb(rtskb);
}
static void rtcfg_client_update_server(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_1_cfg *stage_1_cfg;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
u8 addr_type;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_1_cfg));
addr_type = stage_1_cfg->addr_type;
switch (stage_1_cfg->addr_type) {
#ifdef CONFIG_RTNET_RTIPV4
case RTCFG_ADDR_IP: {
struct rtnet_device *rtdev;
u32 daddr, saddr;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_1_cfg) +
2*RTCFG_ADDRSIZE_IP) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_1_cfg "
"frame\n");
kfree_rtskb(rtskb);
break;
}
rtdev = rtskb->rtdev;
daddr = *(u32*)stage_1_cfg->client_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
saddr = *(u32*)stage_1_cfg->server_addr;
stage_1_cfg = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_cfg) + RTCFG_ADDRSIZE_IP);
__rtskb_pull(rtskb, 2*RTCFG_ADDRSIZE_IP);
/* directed to us? */
if ((rtskb->pkt_type == PACKET_BROADCAST) &&
(daddr != rtdev->local_ip)) {
rtos_res_unlock(&rtcfg_dev->dev_lock);
kfree_rtskb(rtskb);
return;
}
/* update routing table */
rt_ip_route_add_host(saddr, rtskb->mac.ethernet->h_source, rtdev);
rtcfg_dev->spec.clt.srv_addr.ip_addr = saddr;
break;
}
#endif /* CONFIG_RTNET_RTIPV4 */
case RTCFG_ADDR_MAC:
/* nothing to do */
break;
default:
rtos_res_unlock(&rtcfg_dev->dev_lock);
RTCFG_DEBUG(1, "RTcfg: unknown addr_type %d in %s()\n",
stage_1_cfg->addr_type, __FUNCTION__);
kfree_rtskb(rtskb);
return;
}
/* Ethernet-specific */
memcpy(rtcfg_dev->spec.clt.srv_mac_addr,
rtskb->mac.ethernet->h_source, ETH_ALEN);
rtcfg_send_announce_reply(ifindex, rtskb->mac.ethernet->h_source);
rtos_res_unlock(&rtcfg_dev->dev_lock);
kfree_rtskb(rtskb);
}
static void rtcfg_client_recv_stage_2_cfg(int ifindex, struct rtskb *rtskb)
{
struct rtcfg_frm_stage_2_cfg *stage_2_cfg;
struct rtcfg_device *rtcfg_dev = &device[ifindex];
size_t data_len;
int ret;
if (rtskb->len < sizeof(struct rtcfg_frm_stage_2_cfg)) {
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
RTCFG_DEBUG(1, "RTcfg: received invalid stage_2_cfg frame\n");
kfree_rtskb(rtskb);
return;
}
stage_2_cfg = (struct rtcfg_frm_stage_2_cfg *)rtskb->data;
__rtskb_pull(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg));
if (stage_2_cfg->heartbeat_period) {
ret = rtdm_timer_init(&rtcfg_dev->timer, rtcfg_timer, "rtcfg-timer");
if (ret == 0) {
ret = rtdm_timer_start(&rtcfg_dev->timer,
XN_INFINITE,
(nanosecs_rel_t)ntohs(stage_2_cfg->heartbeat_period) *
1000000,
RTDM_TIMERMODE_RELATIVE);
if (ret < 0)
rtdm_timer_destroy(&rtcfg_dev->timer);
}
if (ret < 0)
/*ERRMSG*/rtdm_printk("RTcfg: unable to create timer task\n");
else
set_bit(FLAG_TIMER_STARTED, &rtcfg_dev->flags);
}
/* add server to station list */
if (rtcfg_add_to_station_list(rtcfg_dev,
rtskb->mac.ethernet->h_source, stage_2_cfg->flags) < 0) {
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
RTCFG_DEBUG(1, "RTcfg: unable to process stage_2_cfg frage\n");
kfree_rtskb(rtskb);
return;
}
rtcfg_dev->other_stations = ntohl(stage_2_cfg->stations);
rtcfg_dev->spec.clt.cfg_len = ntohl(stage_2_cfg->cfg_len);
data_len = MIN(rtcfg_dev->spec.clt.cfg_len, rtskb->len);
if (test_bit(RTCFG_FLAG_STAGE_2_DATA, &rtcfg_dev->flags) &&
(data_len > 0)) {
rtcfg_client_queue_frag(ifindex, rtskb, data_len);
rtskb = NULL;
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations)
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_KNOWN);
} else {
if (rtcfg_dev->stations_found == rtcfg_dev->other_stations) {
rtcfg_complete_cmd(ifindex, RTCFG_CMD_ANNOUNCE, 0);
rtcfg_next_main_state(ifindex,
test_bit(RTCFG_FLAG_READY, &rtcfg_dev->flags) ?
RTCFG_MAIN_CLIENT_READY : RTCFG_MAIN_CLIENT_2);
} else
rtcfg_next_main_state(ifindex, RTCFG_MAIN_CLIENT_ALL_FRAMES);
rtcfg_send_ack(ifindex);
}
rtdm_mutex_unlock(&rtcfg_dev->dev_mutex);
if (rtskb != NULL)
kfree_rtskb(rtskb);
}
/***
* rt_udp_recvmsg
*/
ssize_t rt_udp_recvmsg(struct rtdm_dev_context *context, int call_flags,
struct msghdr *msg, int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
struct rtskb *skb;
struct rtskb *first_skb;
size_t copied = 0;
size_t block_size;
size_t data_len;
struct udphdr *uh;
struct sockaddr_in *sin;
int ret;
unsigned long flags;
rtos_time_t timeout;
/* block on receive event */
if (!test_bit(RT_SOCK_NONBLOCK, &context->context_flags) &&
((msg_flags & MSG_DONTWAIT) == 0))
while ((skb = rtskb_dequeue_chain(&sock->incoming)) == NULL) {
rtos_spin_lock_irqsave(&sock->param_lock, flags);
memcpy(&timeout, &sock->timeout, sizeof(timeout));
rtos_spin_unlock_irqrestore(&sock->param_lock, flags);
if (!RTOS_TIME_IS_ZERO(&timeout)) {
ret = rtos_event_sem_wait_timed(&sock->wakeup_event, &timeout);
if (ret == RTOS_EVENT_TIMEOUT)
return -ETIMEDOUT;
} else
ret = rtos_event_sem_wait(&sock->wakeup_event);
if (RTOS_EVENT_ERROR(ret))
return -ENOTSOCK;
}
else {
skb = rtskb_dequeue_chain(&sock->incoming);
if (skb == NULL)
return -EAGAIN;
}
uh = skb->h.uh;
data_len = ntohs(uh->len) - sizeof(struct udphdr);
sin = msg->msg_name;
/* copy the address */
msg->msg_namelen = sizeof(*sin);
if (sin) {
sin->sin_family = AF_INET;
sin->sin_port = uh->source;
sin->sin_addr.s_addr = skb->nh.iph->saddr;
}
/* remove the UDP header */
__rtskb_pull(skb, sizeof(struct udphdr));
first_skb = skb;
/* iterate over all IP fragments */
do {
rtskb_trim(skb, data_len);
block_size = skb->len;
copied += block_size;
data_len -= block_size;
/* The data must not be longer than the available buffer size */
if (copied > len) {
block_size -= copied - len;
copied = len;
msg->msg_flags |= MSG_TRUNC;
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, block_size);
break;
}
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, block_size);
/* next fragment */
skb = skb->next;
} while (skb != NULL);
/* did we copied all bytes? */
if (data_len > 0)
msg->msg_flags |= MSG_TRUNC;
if ((msg_flags & MSG_PEEK) == 0)
kfree_rtskb(first_skb);
else {
__rtskb_push(first_skb, sizeof(struct udphdr));
rtskb_queue_head(&sock->incoming, first_skb);
}
return copied;
}
/***
* rt_ip_local_deliver
*/
static inline int rt_ip_local_deliver(struct rtskb *skb)
{
struct iphdr *iph = skb->nh.iph;
unsigned short protocol = iph->protocol;
struct rtinet_protocol *ipprot;
struct rtsocket *sock;
int ret;
ipprot = rt_inet_protocols[rt_inet_hashkey(protocol)];
/* Check if we are supporting the protocol */
if ((ipprot != NULL) && (ipprot->protocol == protocol))
{
__rtskb_pull(skb, iph->ihl*4);
/* Point into the IP datagram, just past the header. */
skb->h.raw = skb->data;
/* Reassemble IP fragments */
if (iph->frag_off & htons(IP_MF|IP_OFFSET)) {
skb = rt_ip_defrag(skb, ipprot);
if (!skb)
return 0;
} else {
/* Get the destination socket */
if ((sock = ipprot->dest_socket(skb)) == NULL) {
kfree_rtskb(skb);
return 0;
}
/* Acquire the rtskb at the expense of the protocol pool */
ret = rtskb_acquire(skb, &sock->skb_pool);
/* Socket is now implicitely locked by the rtskb */
rt_socket_dereference(sock);
if (ret != 0) {
kfree_rtskb(skb);
return 0;
}
}
/* Deliver the packet to the next layer */
ret = ipprot->rcv_handler(skb);
} else {
#ifdef CONFIG_RTNET_ADDON_PROXY
/* If a fallback handler for IP protocol has been installed,
* call it! */
if (ip_fallback_handler) {
ret = ip_fallback_handler(skb);
if (ret) {
rtos_print("RTnet: fallback handler failed\n");
}
return ret;
}
#endif /* CONFIG_RTNET_ADDON_PROXY */
rtos_print("RTnet: no protocol found\n");
kfree_rtskb(skb);
ret = 0;
}
return ret;
}