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; }