/***
* common reply function
*/
static void rt_icmp_send_reply(struct icmp_bxm *icmp_param, struct rtskb *skb)
{
struct dest_route rt;
int err;
icmp_param->head.icmph.checksum = 0;
icmp_param->csum = 0;
/* route back to the source address via the incoming device */
if (rt_ip_route_output(&rt, skb->nh.iph->saddr,
skb->rtdev->local_ip) != 0)
return;
rt_socket_reference(icmp_socket);
err = rt_ip_build_xmit(icmp_socket, rt_icmp_glue_reply_bits, icmp_param,
sizeof(struct icmphdr) + icmp_param->data_len,
&rt, MSG_DONTWAIT);
if (err)
rt_socket_dereference(icmp_socket);
rtdev_dereference(rt.rtdev);
RTNET_ASSERT(err == 0,
rtdm_printk("RTnet: %s() error in xmit\n", __FUNCTION__););
int rtcfg_send_ready(int ifindex)
{
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_ready *ready_frm;
rtdev = rtdev_get_by_index(ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_ready);
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
ready_frm = (struct rtcfg_frm_ready *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_ready));
ready_frm->head.id = RTCFG_ID_READY;
ready_frm->head.version = 0;
return rtcfg_send_frame(rtskb, rtdev, eth_broadcast);
}
int rtcfg_send_ack(int ifindex)
{
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_ack_cfg *ack_frm;
rtdev = rtdev_get_by_index(ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_ack_cfg);
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
ack_frm = (struct rtcfg_frm_ack_cfg *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_ack_cfg));
ack_frm->head.id = RTCFG_ID_ACK_CFG;
ack_frm->head.version = 0;
ack_frm->ack_len = htonl(device[ifindex].cfg_offs);
return rtcfg_send_frame(rtskb, rtdev, device[ifindex].srv_mac_addr);
}
static void rtmac_vnic_signal_handler(rtdm_nrtsig_t nrtsig, void *arg)
{
struct rtskb *rtskb;
struct sk_buff *skb;
unsigned hdrlen;
struct net_device_stats *stats;
struct rtnet_device *rtdev;
while (1)
{
rtskb = rtskb_dequeue(&rx_queue);
if (!rtskb)
break;
rtdev = rtskb->rtdev;
hdrlen = rtdev->hard_header_len;
skb = dev_alloc_skb(hdrlen + rtskb->len + 2);
if (skb) {
/* the rtskb stamp is useless (different clock), get new one */
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,14)
__net_timestamp(skb);
#else
do_gettimeofday(&skb->stamp);
#endif
skb_reserve(skb, 2); /* Align IP on 16 byte boundaries */
/* copy Ethernet header */
memcpy(skb_put(skb, hdrlen),
rtskb->data - hdrlen - sizeof(struct rtmac_hdr), hdrlen);
/* patch the protocol field in the original Ethernet header */
((struct ethhdr*)skb->data)->h_proto = rtskb->protocol;
/* copy data */
memcpy(skb_put(skb, rtskb->len), rtskb->data, rtskb->len);
skb->dev = rtskb->rtdev->mac_priv->vnic;
skb->protocol = eth_type_trans(skb, skb->dev);
stats = &rtskb->rtdev->mac_priv->vnic_stats;
kfree_rtskb(rtskb);
stats->rx_packets++;
stats->rx_bytes += skb->len;
netif_rx(skb);
}
else {
printk("RTmac: VNIC fails to allocate linux skb\n");
kfree_rtskb(rtskb);
}
rtdev_dereference(rtdev);
}
}
/***
* rt_loopback_xmit - begin packet transmission
* @skb: packet to be sent
* @dev: network device to which packet is sent
*
*/
static int rt_loopback_xmit(struct rtskb *skb, struct rtnet_device *rtdev)
{
unsigned short hash;
struct rtpacket_type *pt_entry;
unsigned long flags;
rtos_time_t time;
/* write transmission stamp - in case any protocol ever gets the idea to
ask the lookback device for this service... */
if (skb->xmit_stamp) {
rtos_get_time(&time);
*skb->xmit_stamp =
cpu_to_be64(rtos_time_to_nanosecs(&time) + *skb->xmit_stamp);
}
/* make sure that critical fields are re-intialised */
skb->chain_end = skb;
/* parse the Ethernet header as usual */
skb->protocol = rt_eth_type_trans(skb, rtdev);
skb->nh.raw = skb->data;
rtdev_reference(rtdev);
rtcap_report_incoming(skb);
hash = ntohs(skb->protocol) & RTPACKET_HASH_KEY_MASK;
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
list_for_each_entry(pt_entry, &rt_packets[hash], list_entry)
if (pt_entry->type == skb->protocol) {
pt_entry->refcount++;
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
pt_entry->handler(skb, pt_entry);
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
pt_entry->refcount--;
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
goto out;
}
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
/* don't warn if running in promiscuous mode (RTcap...?) */
if ((rtdev->flags & IFF_PROMISC) == 0)
rtos_print("RTnet: unknown layer-3 protocol\n");
kfree_rtskb(skb);
out:
rtdev_dereference(rtdev);
return 0;
}
static int rt_host_route_read_proc(char *buf, char **start, off_t offset,
int count, int *eof, void *data)
{
struct host_route *entry_ptr;
struct dest_route dest_host;
unsigned int key;
unsigned int index;
unsigned int i;
rtdm_lockctx_t context;
int res;
RTNET_PROC_PRINT_VARS_EX(80);
if (!RTNET_PROC_PRINT_EX("Hash\tDestination\tHW Address\t\tDevice\n"))
goto done;
for (key = 0; key < HOST_HASH_TBL_SIZE; key++) {
index = 0;
while (1) {
rtdm_lock_get_irqsave(&host_table_lock, context);
entry_ptr = host_hash_tbl[key];
for (i = 0; (i < index) && (entry_ptr != NULL); i++)
entry_ptr = entry_ptr->next;
if (entry_ptr == NULL) {
rtdm_lock_put_irqrestore(&host_table_lock, context);
break;
}
memcpy(&dest_host, &entry_ptr->dest_host,
sizeof(struct dest_route));
rtdev_reference(dest_host.rtdev);
rtdm_lock_put_irqrestore(&host_table_lock, context);
res = RTNET_PROC_PRINT_EX("%02X\t%u.%u.%u.%-3u\t"
"%02X:%02X:%02X:%02X:%02X:%02X\t%s\n",
key, NIPQUAD(dest_host.ip),
dest_host.dev_addr[0], dest_host.dev_addr[1],
dest_host.dev_addr[2], dest_host.dev_addr[3],
dest_host.dev_addr[4], dest_host.dev_addr[5],
dest_host.rtdev->name);
rtdev_dereference(dest_host.rtdev);
if (!res)
goto done;
index++;
}
}
done:
RTNET_PROC_PRINT_DONE_EX;
}
int rtcfg_send_dead_station(struct rtcfg_connection *conn)
{
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_dead_station *dead_station_frm;
rtdev = rtdev_get_by_index(conn->ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len +
sizeof(struct rtcfg_frm_dead_station) +
#ifdef CONFIG_RTNET_RTIPV4
(((conn->addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) ?
RTCFG_ADDRSIZE_IP : 0);
#else /* !CONFIG_RTNET_RTIPV4 */
0;
#endif /* CONFIG_RTNET_RTIPV4 */
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
dead_station_frm = (struct rtcfg_frm_dead_station *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_dead_station));
dead_station_frm->head.id = RTCFG_ID_DEAD_STATION;
dead_station_frm->head.version = 0;
dead_station_frm->addr_type = conn->addr_type & RTCFG_ADDR_MASK;
#ifdef CONFIG_RTNET_RTIPV4
if (dead_station_frm->addr_type == RTCFG_ADDR_IP) {
rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);
memcpy(dead_station_frm->logical_addr, &(conn->addr.ip_addr), 4);
dead_station_frm = (struct rtcfg_frm_dead_station *)
(((u8 *)dead_station_frm) + RTCFG_ADDRSIZE_IP);
}
#endif /* CONFIG_RTNET_RTIPV4 */
/* Ethernet-specific! */
memcpy(dead_station_frm->physical_addr, conn->mac_addr, ETH_ALEN);
memset(&dead_station_frm->physical_addr[ETH_ALEN], 0,
sizeof(dead_station_frm->physical_addr) - ETH_ALEN);
return rtcfg_send_frame(rtskb, rtdev, rtdev->broadcast);
}
/***
* rt_loopback_xmit - begin packet transmission
* @skb: packet to be sent
* @dev: network device to which packet is sent
*
*/
static int rt_loopback_xmit(struct rtskb *skb, struct rtnet_device *rtdev)
{
unsigned short hash;
struct rtpacket_type *pt;
unsigned long flags;
rtos_time_t time;
/* write transmission stamp - in case any protocol ever gets the idea to
ask the lookback device for this service... */
if (skb->xmit_stamp) {
rtos_get_time(&time);
*skb->xmit_stamp =
cpu_to_be64(rtos_time_to_nanosecs(&time) + *skb->xmit_stamp);
}
/* make sure that critical fields are re-intialised */
skb->chain_end = skb;
/* parse the Ethernet header as usual */
skb->protocol = rt_eth_type_trans(skb, rtdev);
skb->nh.raw = skb->data;
rtdev_reference(rtdev);
rtcap_report_incoming(skb);
hash = ntohs(skb->protocol) & (MAX_RT_PROTOCOLS-1);
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
pt = rt_packets[hash];
if ((pt != NULL) && (pt->type == skb->protocol)) {
pt->refcount++;
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
pt->handler(skb, pt);
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
pt->refcount--;
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
} else {
rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
rtos_print("RTnet: unknown layer-3 protocol\n");
kfree_rtskb(skb);
}
rtdev_dereference(rtdev);
return 0;
}
int rtcfg_send_stage_1(struct rtcfg_connection *conn)
{
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_stage_1_cfg *stage_1_frm;
rtdev = rtdev_get_by_index(conn->ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len +
sizeof(struct rtcfg_frm_stage_1_cfg) + conn->stage1_size +
(((conn->addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) ?
2*RTCFG_ADDRSIZE_IP : 0);
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
stage_1_frm = (struct rtcfg_frm_stage_1_cfg *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_stage_1_cfg));
stage_1_frm->head.id = RTCFG_ID_STAGE_1_CFG;
stage_1_frm->head.version = 0;
stage_1_frm->addr_type = conn->addr_type & RTCFG_ADDR_MASK;
if (stage_1_frm->addr_type == RTCFG_ADDR_IP) {
rtskb_put(rtskb, 2*RTCFG_ADDRSIZE_IP);
*(u32*)stage_1_frm->client_addr = conn->addr.ip_addr;
stage_1_frm = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_frm) + RTCFG_ADDRSIZE_IP);
*(u32*)stage_1_frm->server_addr = rtdev->local_ip;
stage_1_frm = (struct rtcfg_frm_stage_1_cfg *)
(((u8 *)stage_1_frm) + RTCFG_ADDRSIZE_IP);
}
stage_1_frm->burstrate = device[conn->ifindex].burstrate;
stage_1_frm->cfg_len = htons(conn->stage1_size);
memcpy(rtskb_put(rtskb, conn->stage1_size), conn->stage1_data,
conn->stage1_size);
return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}
int rtcfg_send_announce_reply(int ifindex, u8 *dest_mac_addr)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_announce *announce_rpl;
rtdev = rtdev_get_by_index(ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len +
sizeof(struct rtcfg_frm_announce) +
#ifdef CONFIG_RTNET_RTIPV4
((rtcfg_dev->spec.clt.addr_type == RTCFG_ADDR_IP) ?
RTCFG_ADDRSIZE_IP : 0);
#else /* !CONFIG_RTNET_RTIPV4 */
0;
#endif /* CONFIG_RTNET_RTIPV4 */
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
announce_rpl = (struct rtcfg_frm_announce *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_announce));
announce_rpl->head.id = RTCFG_ID_ANNOUNCE_REPLY;
announce_rpl->head.version = 0;
announce_rpl->addr_type = rtcfg_dev->spec.clt.addr_type;
#ifdef CONFIG_RTNET_RTIPV4
if (announce_rpl->addr_type == RTCFG_ADDR_IP) {
rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);
memcpy(announce_rpl->addr, &(rtdev->local_ip), 4);
announce_rpl = (struct rtcfg_frm_announce *)
(((u8 *)announce_rpl) + RTCFG_ADDRSIZE_IP);
}
#endif /* CONFIG_RTNET_RTIPV4 */
announce_rpl->flags = rtcfg_dev->flags & RTCFG_FLAG_READY;
announce_rpl->burstrate = 0; /* padding field */
return rtcfg_send_frame(rtskb, rtdev, dest_mac_addr);
}
int rtcfg_send_stage_2(struct rtcfg_connection *conn, int send_data)
{
struct rtnet_device *rtdev;
struct rtcfg_device *rtcfg_dev = &device[conn->ifindex];
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_stage_2_cfg *stage_2_frm;
size_t total_size;
size_t frag_size;
rtdev = rtdev_get_by_index(conn->ifindex);
if (rtdev == NULL)
return -ENODEV;
if (send_data) {
total_size = conn->stage2_file->size;
frag_size = MIN(rtdev->mtu - sizeof(struct rtcfg_frm_stage_2_cfg),
total_size);
} else {
total_size = 0;
frag_size = 0;
}
rtskb_size = rtdev->hard_header_len +
sizeof(struct rtcfg_frm_stage_2_cfg) + frag_size;
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
stage_2_frm = (struct rtcfg_frm_stage_2_cfg *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg));
stage_2_frm->head.id = RTCFG_ID_STAGE_2_CFG;
stage_2_frm->head.version = 0;
stage_2_frm->flags = rtcfg_dev->flags;
stage_2_frm->stations = htonl(rtcfg_dev->other_stations);
stage_2_frm->heartbeat_period = htons(0);
stage_2_frm->cfg_len = htonl(total_size);
if (send_data)
memcpy(rtskb_put(rtskb, frag_size), conn->stage2_file->buffer,
frag_size);
conn->cfg_offs = frag_size;
return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}
int rtcfg_send_announce_new(int ifindex)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_announce *announce_new;
rtdev = rtdev_get_by_index(ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_announce) +
#ifdef CONFIG_RTNET_RTIPV4
(((rtcfg_dev->spec.clt.addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) ?
RTCFG_ADDRSIZE_IP : 0);
#else /* !CONFIG_RTNET_RTIPV4 */
0;
#endif /* CONFIG_RTNET_RTIPV4 */
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
announce_new = (struct rtcfg_frm_announce *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_announce));
announce_new->head.id = RTCFG_ID_ANNOUNCE_NEW;
announce_new->head.version = 0;
announce_new->addr_type = rtcfg_dev->spec.clt.addr_type;
#ifdef CONFIG_RTNET_RTIPV4
if (announce_new->addr_type == RTCFG_ADDR_IP) {
rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);
memcpy(announce_new->addr, &(rtdev->local_ip), 4);
announce_new = (struct rtcfg_frm_announce *)
(((u8 *)announce_new) + RTCFG_ADDRSIZE_IP);
}
#endif /* CONFIG_RTNET_RTIPV4 */
announce_new->flags = rtcfg_dev->flags;
announce_new->burstrate = rtcfg_dev->burstrate;
return rtcfg_send_frame(rtskb, rtdev, rtdev->broadcast);
}
static void rtcfg_rx_task(int arg)
{
struct rtskb *rtskb;
struct rtcfg_frm_head *frm_head;
struct rtnet_device *rtdev;
while (1) {
if (RTOS_EVENT_ERROR(rtos_event_sem_wait(&rx_event)))
return;
rtskb = rtskb_dequeue(&rx_queue);
rtdev = rtskb->rtdev;
if (rtskb->pkt_type == PACKET_OTHERHOST) {
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
if (rtskb->len < sizeof(struct rtcfg_frm_head)) {
RTCFG_DEBUG(1, "RTcfg: %s() received an invalid frame\n",
__FUNCTION__);
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
frm_head = (struct rtcfg_frm_head *)rtskb->data;
if (rtcfg_do_main_event(rtskb->rtdev->ifindex,
frm_head->id + RTCFG_FRM_STAGE_1_CFG,
rtskb) < 0)
kfree_rtskb(rtskb);
rtdev_dereference(rtdev);
}
}
static int rt_host_route_read_proc(char *buf, char **start, off_t offset,
int count, int *eof, void *data)
{
RTNET_PROC_PRINT_VARS;
struct host_route *entry_ptr;
struct dest_route dest_host;
unsigned int key;
unsigned int index;
unsigned int i;
unsigned long flags;
RTNET_PROC_PRINT("Hash\tDestination\tHW Address\t\tDevice\n");
for (key = 0; key < HOST_HASH_TBL_SIZE; key++) {
index = 0;
while (1) {
rtos_spin_lock_irqsave(&host_table_lock, flags);
entry_ptr = host_table[key];
for (i = 0; (i < index) && (entry_ptr != NULL); i++)
entry_ptr = entry_ptr->next;
if (entry_ptr == NULL) {
rtos_spin_unlock_irqrestore(&host_table_lock, flags);
break;
}
memcpy(&dest_host, &entry_ptr->dest_host,
sizeof(struct dest_route));
rtdev_reference(dest_host.rtdev);
rtos_spin_unlock_irqrestore(&host_table_lock, flags);
RTNET_PROC_PRINT("%02X\t%u.%u.%u.%-3u\t"
"%02X:%02X:%02X:%02X:%02X:%02X\t%s\n",
key, NIPQUAD(dest_host.ip),
dest_host.dev_addr[0], dest_host.dev_addr[1],
dest_host.dev_addr[2], dest_host.dev_addr[3],
dest_host.dev_addr[4], dest_host.dev_addr[5],
dest_host.rtdev->name);
rtdev_dereference(dest_host.rtdev);
index++;
}
}
RTNET_PROC_PRINT_DONE;
}
/***
* rtdev_alloc_name - allocate a name for the rtnet_device
* @rtdev: the rtnet_device
* @name_mask: a name mask (e.g. "rteth%d" for ethernet)
*
* This function have to be called from the driver probe function.
*/
void rtdev_alloc_name(struct rtnet_device *rtdev, const char *mask)
{
char buf[IFNAMSIZ];
int i;
struct rtnet_device *tmp;
for (i = 0; i < MAX_RT_DEVICES; i++) {
snprintf(buf, IFNAMSIZ, mask, i);
if ((tmp = rtdev_get_by_name(buf)) == NULL) {
strncpy(rtdev->name, buf, IFNAMSIZ);
break;
}
else
rtdev_dereference(tmp);
}
}
int rtcfg_send_announce_new(int ifindex)
{
struct rtcfg_device *rtcfg_dev = &device[ifindex];
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_announce *announce_new;
rtdev = rtdev_get_by_index(ifindex);
if (rtdev == NULL)
return -ENODEV;
rtskb_size = rtdev->hard_header_len + sizeof(struct rtcfg_frm_announce) +
((rtcfg_dev->addr_type == RTCFG_ADDR_IP) ? RTCFG_ADDRSIZE_IP : 0);
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
announce_new = (struct rtcfg_frm_announce *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_announce));
announce_new->head.id = RTCFG_ID_ANNOUNCE_NEW;
announce_new->head.version = 0;
announce_new->addr_type = rtcfg_dev->addr_type;
if (announce_new->addr_type == RTCFG_ADDR_IP) {
rtskb_put(rtskb, RTCFG_ADDRSIZE_IP);
*(u32*)announce_new->addr = rtdev->local_ip;
announce_new = (struct rtcfg_frm_announce *)
(((u8 *)announce_new) + RTCFG_ADDRSIZE_IP);
}
announce_new->flags = rtcfg_dev->flags;
announce_new->burstrate = rtcfg_dev->burstrate;
return rtcfg_send_frame(rtskb, rtdev, rtdev->broadcast);
}
int rtcfg_send_stage_2_frag(struct rtcfg_connection *conn)
{
struct rtnet_device *rtdev;
struct rtskb *rtskb;
unsigned int rtskb_size;
struct rtcfg_frm_stage_2_cfg_frag *stage_2_frm;
size_t frag_size;
rtdev = rtdev_get_by_index(conn->ifindex);
if (rtdev == NULL)
return -ENODEV;
frag_size = MIN(rtdev->get_mtu(rtdev, RTCFG_SKB_PRIO) -
sizeof(struct rtcfg_frm_stage_2_cfg_frag),
conn->stage2_file->size - conn->cfg_offs);
rtskb_size = rtdev->hard_header_len +
sizeof(struct rtcfg_frm_stage_2_cfg_frag) + frag_size;
rtskb = alloc_rtskb(rtskb_size, &rtcfg_pool);
if (rtskb == NULL) {
rtdev_dereference(rtdev);
return -ENOBUFS;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
stage_2_frm = (struct rtcfg_frm_stage_2_cfg_frag *)
rtskb_put(rtskb, sizeof(struct rtcfg_frm_stage_2_cfg_frag));
stage_2_frm->head.id = RTCFG_ID_STAGE_2_CFG_FRAG;
stage_2_frm->head.version = 0;
stage_2_frm->frag_offs = htonl(conn->cfg_offs);
memcpy(rtskb_put(rtskb, frag_size),
conn->stage2_file->buffer + conn->cfg_offs, frag_size);
conn->cfg_offs += frag_size;
return rtcfg_send_frame(rtskb, rtdev, conn->mac_addr);
}
static void rtcfg_conn_check_heartbeat(struct rtcfg_connection *conn)
{
u64 timeout;
struct rtcfg_device *rtcfg_dev;
timeout = device[conn->ifindex].spec.srv.heartbeat_timeout;
if (!timeout)
return;
if (rtdm_clock_read() >= conn->last_frame + timeout) {
rtcfg_dev = &device[conn->ifindex];
rtcfg_dev->stations_found--;
rtcfg_dev->stations_ready--;
rtcfg_dev->spec.srv.clients_configured--;
rtcfg_send_dead_station(conn);
rtcfg_next_conn_state(conn, RTCFG_CONN_DEAD);
conn->cfg_offs = 0;
conn->flags = 0;
#ifdef CONFIG_RTNET_RTIPV4
if ((conn->addr_type & RTCFG_ADDR_MASK) == RTCFG_ADDR_IP) {
struct rtnet_device *rtdev = rtdev_get_by_index(conn->ifindex);
rt_ip_route_del_host(conn->addr.ip_addr, rtdev);
if (rtdev == NULL)
return;
if (!(conn->addr_type & FLAG_ASSIGN_ADDR_BY_MAC))
/* MAC address yet unknown -> use broadcast address */
memcpy(conn->mac_addr, rtdev->broadcast, MAX_ADDR_LEN);
rtdev_dereference(rtdev);
}
#endif /* CONFIG_RTNET_RTIPV4 */
}
}
void rtcfg_cleanup_frames(void)
{
struct rtskb *rtskb;
while (rtdev_remove_pack(&rtcfg_packet_type) == -EAGAIN) {
RTCFG_DEBUG(3, "RTcfg: waiting for protocol unregistration\n");
set_current_state(TASK_UNINTERRUPTIBLE);
schedule_timeout(1*HZ); /* wait a second */
}
rtos_event_sem_delete(&rx_event);
rtos_task_delete(&rx_task);
while ((rtskb = rtskb_dequeue(&rx_queue)) != NULL) {
rtdev_dereference(rtskb->rtdev);
kfree_rtskb(rtskb);
}
rtskb_pool_release(&rtcfg_pool);
}
/***
* common reply function
*/
static void rt_icmp_send_reply(struct icmp_bxm *icmp_param, struct rtskb *skb)
{
struct dest_route rt;
u32 daddr;
int err;
daddr = skb->nh.iph->saddr;
icmp_param->head.icmph.checksum = 0;
icmp_param->csum = 0;
if (rt_ip_route_output(&rt, daddr) != 0)
return;
err = rt_ip_build_xmit(&icmp_socket, rt_icmp_glue_reply_bits, icmp_param,
sizeof(struct icmphdr) + icmp_param->data_len,
&rt, MSG_DONTWAIT);
rtdev_dereference(rt.rtdev);
RTNET_ASSERT(err == 0,
rtos_print("RTnet: %s() error in xmit\n", __FUNCTION__););
/***
* rt_packet_getsockname
*/
int rt_packet_getsockname(struct rtsocket *sock, struct sockaddr *addr,
socklen_t *addrlen)
{
struct sockaddr_ll *sll = (struct sockaddr_ll*)addr;
struct rtnet_device *rtdev;
rtdm_lockctx_t context;
if (*addrlen < sizeof(struct sockaddr_ll))
return -EINVAL;
rtdm_lock_get_irqsave(&sock->param_lock, context);
sll->sll_family = AF_PACKET;
sll->sll_ifindex = sock->prot.packet.ifindex;
sll->sll_protocol = sock->protocol;
rtdm_lock_put_irqrestore(&sock->param_lock, context);
rtdev = rtdev_get_by_index(sll->sll_ifindex);
if (rtdev != NULL) {
sll->sll_hatype = rtdev->type;
sll->sll_halen = rtdev->addr_len;
memcpy(sll->sll_addr, rtdev->dev_addr, rtdev->addr_len);
rtdev_dereference(rtdev);
} else {
sll->sll_hatype = 0;
sll->sll_halen = 0;
}
*addrlen = sizeof(struct sockaddr_ll);
return 0;
}
/***
* rt_socket_if_ioctl
*/
int rt_socket_if_ioctl(struct rtdm_dev_context *context, int call_flags,
int request, void *arg)
{
struct rtnet_device *rtdev;
struct ifreq *cur_ifr;
struct sockaddr_in *sin;
int i;
int size;
struct ifconf *ifc = arg;
struct ifreq *ifr = arg;
int ret = 0;
switch (request) {
case SIOCGIFCONF:
size = 0;
cur_ifr = ifc->ifc_req;
for (i = 1; i <= MAX_RT_DEVICES; i++) {
rtdev = rtdev_get_by_index(i);
if (rtdev != NULL) {
if ((rtdev->flags & IFF_RUNNING) == 0) {
rtdev_dereference(rtdev);
continue;
}
size += sizeof(struct ifreq);
if (size > ifc->ifc_len) {
rtdev_dereference(rtdev);
size = ifc->ifc_len;
break;
}
strncpy(cur_ifr->ifr_name, rtdev->name,
IFNAMSIZ);
sin = (struct sockaddr_in *)&cur_ifr->ifr_addr;
sin->sin_family = AF_INET;
sin->sin_addr.s_addr = rtdev->local_ip;
cur_ifr++;
rtdev_dereference(rtdev);
}
}
ifc->ifc_len = size;
break;
case SIOCGIFFLAGS:
rtdev = rtdev_get_by_name(ifr->ifr_name);
if (rtdev == NULL)
return -ENODEV;
else {
ifr->ifr_flags = rtdev->flags;
rtdev_dereference(rtdev);
}
break;
default:
ret = -EOPNOTSUPP;
break;
}
return ret;
}
/***
* rt_socket_if_ioctl
*/
int rt_socket_if_ioctl(struct rtdm_dev_context *sockctx,
rtdm_user_info_t *user_info, int request, void *arg)
{
struct rtnet_device *rtdev;
struct ifreq *ifr = arg;
struct sockaddr_in *sin;
int ret = 0;
if (request == SIOCGIFCONF) {
struct ifconf *ifc = arg;
struct ifreq *cur_ifr = ifc->ifc_req;
int size = 0;
int i;
for (i = 1; i <= MAX_RT_DEVICES; i++) {
rtdev = rtdev_get_by_index(i);
if (rtdev != NULL) {
if ((rtdev->flags & IFF_RUNNING) == 0) {
rtdev_dereference(rtdev);
continue;
}
size += sizeof(struct ifreq);
if (size > ifc->ifc_len) {
rtdev_dereference(rtdev);
size = ifc->ifc_len;
break;
}
strncpy(cur_ifr->ifr_name, rtdev->name,
IFNAMSIZ);
sin = (struct sockaddr_in *)&cur_ifr->ifr_addr;
sin->sin_family = AF_INET;
sin->sin_addr.s_addr = rtdev->local_ip;
cur_ifr++;
rtdev_dereference(rtdev);
}
}
ifc->ifc_len = size;
return 0;
}
rtdev = rtdev_get_by_name(ifr->ifr_name);
if (rtdev == NULL)
return -ENODEV;
switch (request) {
case SIOCGIFINDEX:
ifr->ifr_ifindex = rtdev->ifindex;
break;
case SIOCGIFFLAGS:
ifr->ifr_flags = rtdev->flags;
break;
case SIOCGIFHWADDR:
memcpy(ifr->ifr_hwaddr.sa_data, rtdev->dev_addr, rtdev->addr_len);
ifr->ifr_hwaddr.sa_family = rtdev->type;
break;
case SIOCGIFADDR:
sin = (struct sockaddr_in *)&ifr->ifr_addr;
sin->sin_family = AF_INET;
sin->sin_addr.s_addr = rtdev->local_ip;
break;
case SIOCETHTOOL:
if (rtdev->do_ioctl != NULL)
ret = rtdev->do_ioctl(rtdev, request, arg);
else
ret = -EOPNOTSUPP;
break;
default:
ret = -EOPNOTSUPP;
break;
}
rtdev_dereference(rtdev);
return ret;
}
int rtwlan_tx(struct rtskb *rtskb, struct rtnet_device *rtnet_dev)
{
struct rtwlan_device * rtwlan = rtnetdev_priv(rtnet_dev);
struct ieee80211_hdr_3addr header = { /* Ensure zero initialized */
.duration_id = 0,
.seq_ctl = 0
};
u8 dest[ETH_ALEN], src[ETH_ALEN];
/* Save source and destination addresses */
memcpy(dest, rtskb->data, ETH_ALEN);
memcpy(src, rtskb->data + ETH_ALEN, ETH_ALEN);
/* Generate ieee80211 compatible header */
memcpy(header.addr3, src, ETH_ALEN); /* BSSID */
memcpy(header.addr2, src, ETH_ALEN); /* SA */
memcpy(header.addr1, dest, ETH_ALEN); /* DA */
/* Frame Control */
switch(rtwlan->mode) {
case RTWLAN_MODE_RAW:
header.frame_ctl = cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
break;
case RTWLAN_MODE_ACK:
header.frame_ctl = cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA);
break;
default:
return -1;
}
memcpy(rtskb_push(rtskb, IEEE80211_3ADDR_LEN), &header, IEEE80211_3ADDR_LEN);
return 0;
}
EXPORT_SYMBOL(rtwlan_tx);
/**
* rtalloc_wlandev - Allocates and sets up a wlan device
* @sizeof_priv: size of additional driver-private structure to
* be allocated for this wlan device
*
* Fill in the fields of the device structure with wlan-generic
* values. Basically does everything except registering the device.
*
* A 32-byte alignment is enforced for the private data area.
*/
struct rtnet_device * rtwlan_alloc_dev(int sizeof_priv)
{
struct rtnet_device *rtnet_dev;
RTWLAN_DEBUG("Start.\n");
rtnet_dev = rt_alloc_etherdev(sizeof(struct rtwlan_device) + sizeof_priv);
if (!rtnet_dev)
return NULL;
rtdev_alloc_name(rtnet_dev, "rtwlan%d");
return rtnet_dev;
}
EXPORT_SYMBOL(rtwlan_alloc_dev);
int rtwlan_ioctl(struct rtnet_device * rtdev,
unsigned int request,
unsigned long arg)
{
struct rtwlan_cmd cmd;
int ret=0;
if (copy_from_user(&cmd, (void *)arg, sizeof(cmd)) != 0)
return -EFAULT;
switch(request) {
case IOC_RTWLAN_IFINFO:
if (cmd.args.info.ifindex > 0)
rtdev = rtdev_get_by_index(cmd.args.info.ifindex);
else
rtdev = rtdev_get_by_name(cmd.head.if_name);
if (rtdev == NULL)
return -ENODEV;
if (down_interruptible(&rtdev->nrt_lock)) {
rtdev_dereference(rtdev);
return -ERESTARTSYS;
}
if (rtdev->do_ioctl)
ret = rtdev->do_ioctl(rtdev, request, &cmd);
else
ret = -ENORTWLANDEV;
memcpy(cmd.head.if_name, rtdev->name, IFNAMSIZ);
cmd.args.info.ifindex = rtdev->ifindex;
cmd.args.info.flags = rtdev->flags;
up(&rtdev->nrt_lock);
rtdev_dereference(rtdev);
break;
case IOC_RTWLAN_MODE:
case IOC_RTWLAN_BITRATE:
case IOC_RTWLAN_CHANNEL:
case IOC_RTWLAN_TXPOWER:
case IOC_RTWLAN_DROPBCAST:
case IOC_RTWLAN_DROPMCAST:
case IOC_RTWLAN_REGREAD:
case IOC_RTWLAN_REGWRITE:
case IOC_RTWLAN_BBPWRITE:
case IOC_RTWLAN_BBPREAD:
case IOC_RTWLAN_BBPSENS:
if (down_interruptible(&rtdev->nrt_lock))
return -ERESTARTSYS;
if (rtdev->do_ioctl)
ret = rtdev->do_ioctl(rtdev, request, &cmd);
else
ret = -ENORTWLANDEV;
up(&rtdev->nrt_lock);
break;
default:
ret = -ENOTTY;
}
if (copy_to_user((void *)arg, &cmd, sizeof(cmd)) != 0)
return -EFAULT;
return ret;
}
struct rtnet_ioctls rtnet_wlan_ioctls = {
service_name: "rtwlan ioctl",
ioctl_type: RTNET_IOC_TYPE_RTWLAN,
handler: rtwlan_ioctl
};
/***
* rt_udp_sendmsg
*/
ssize_t rt_udp_sendmsg(struct rtdm_dev_context *context, int call_flags,
const 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);
int ulen = len + sizeof(struct udphdr);
struct sockaddr_in *usin;
struct udpfakehdr ufh;
struct dest_route rt;
u32 saddr;
u32 daddr;
u16 dport;
int err;
unsigned long flags;
if ((len < 0) || (len > 0xFFFF-sizeof(struct iphdr)-sizeof(struct udphdr)))
return -EMSGSIZE;
if (msg_flags & MSG_OOB) /* Mirror BSD error message compatibility */
return -EOPNOTSUPP;
if (msg_flags & ~(MSG_DONTROUTE|MSG_DONTWAIT) )
return -EINVAL;
if ((msg->msg_name) && (msg->msg_namelen==sizeof(struct sockaddr_in))) {
usin = (struct sockaddr_in*) msg->msg_name;
if ((usin->sin_family != AF_INET) && (usin->sin_family != AF_UNSPEC))
return -EINVAL;
daddr = usin->sin_addr.s_addr;
dport = usin->sin_port;
rtos_spin_lock_irqsave(&sock->param_lock, flags);
} else {
rtos_spin_lock_irqsave(&sock->param_lock, flags);
if (sock->prot.inet.state != TCP_ESTABLISHED)
return -ENOTCONN;
daddr = sock->prot.inet.daddr;
dport = sock->prot.inet.dport;
}
saddr = sock->prot.inet.saddr;
ufh.uh.source = sock->prot.inet.sport;
rtos_spin_unlock_irqrestore(&sock->param_lock, flags);
if ((daddr | dport) == 0)
return -EINVAL;
/* get output route */
err = rt_ip_route_output(&rt, daddr);
if (err)
return err;
/* check if specified source address fits */
if ((saddr != INADDR_ANY) && (saddr != rt.rtdev->local_ip)) {
rtdev_dereference(rt.rtdev);
return -EHOSTUNREACH;
}
/* we found a route, remember the routing dest-addr could be the netmask */
ufh.saddr = rt.rtdev->local_ip;
ufh.daddr = daddr;
ufh.uh.dest = dport;
ufh.uh.len = htons(ulen);
ufh.uh.check = 0;
ufh.iov = msg->msg_iov;
ufh.iovlen = msg->msg_iovlen;
ufh.wcheck = 0;
err = rt_ip_build_xmit(sock, rt_udp_getfrag, &ufh, ulen, &rt, msg_flags);
rtdev_dereference(rt.rtdev);
if (!err)
return len;
else
return err;
}
/***
* rt_packet_recvmsg
*/
ssize_t rt_packet_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);
size_t copy_len;
size_t real_len;
struct rtskb *skb;
struct ethhdr *eth;
struct sockaddr_ll *sll;
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;
}
eth = skb->mac.ethernet;
sll = msg->msg_name;
/* copy the address */
msg->msg_namelen = sizeof(*sll);
if (sll != NULL) {
sll->sll_family = AF_PACKET;
sll->sll_protocol = skb->protocol;
sll->sll_ifindex = skb->rtdev->ifindex;
sll->sll_pkttype = skb->pkt_type;
/* Ethernet specific */
sll->sll_hatype = ARPHRD_ETHER;
sll->sll_halen = ETH_ALEN;
memcpy(sll->sll_addr, eth->h_source, ETH_ALEN);
}
copy_len = real_len = skb->len;
/* The data must not be longer than the available buffer size */
if (copy_len > len) {
copy_len = len;
msg->msg_flags |= MSG_TRUNC;
}
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, copy_len);
if ((msg_flags & MSG_PEEK) == 0) {
rtdev_dereference(skb->rtdev);
kfree_rtskb(skb);
} else
rtskb_queue_head(&sock->incoming, skb);
return real_len;
}
/***
* rt_packet_recvmsg
*/
int rt_packet_recvmsg(struct rtsocket *sock, struct msghdr *msg, size_t len,
int flags)
{
size_t copy_len, real_len;
struct rtskb *skb;
struct ethhdr *eth;
struct sockaddr_ll *sll;
int ret;
/* block on receive event */
if (((sock->flags & RT_SOCK_NONBLOCK) == 0) &&
((flags & MSG_DONTWAIT) == 0))
while ((skb = rtskb_dequeue_chain(&sock->incoming)) == NULL) {
if (RTOS_TIME_IS_ZERO(&sock->timeout)) {
ret = rtos_event_wait_timeout(&sock->wakeup_event,
&sock->timeout);
if (ret == RTOS_EVENT_TIMEOUT)
return -ETIMEDOUT;
} else
ret = rtos_event_wait(&sock->wakeup_event);
if (RTOS_EVENT_ERROR(ret))
return -ENOTSOCK;
}
else {
skb = rtskb_dequeue_chain(&sock->incoming);
if (skb == NULL)
return 0;
}
eth = skb->mac.ethernet;
sll = msg->msg_name;
/* copy the address */
msg->msg_namelen = sizeof(*sll);
if (sll != NULL) {
sll->sll_family = AF_PACKET;
sll->sll_protocol = skb->protocol;
sll->sll_ifindex = skb->rtdev->ifindex;
sll->sll_pkttype = skb->pkt_type;
/* Ethernet specific */
sll->sll_hatype = ARPHRD_ETHER;
sll->sll_halen = ETH_ALEN;
memcpy(sll->sll_addr, eth->h_source, ETH_ALEN);
}
copy_len = real_len = skb->len;
/* The data must not be longer than the available buffer size */
if (copy_len > len) {
copy_len = len;
msg->msg_flags |= MSG_TRUNC;
}
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, copy_len);
if ((flags & MSG_PEEK) == 0) {
rtdev_dereference(skb->rtdev);
kfree_rtskb(skb);
} else
rtskb_queue_head(&sock->incoming, skb);
return real_len;
}
/***
* rt_packet_sendmsg
*/
int rt_packet_sendmsg(struct rtsocket *sock, const struct msghdr *msg,
size_t len, int flags)
{
struct sockaddr_ll *sll = (struct sockaddr_ll*)msg->msg_name;
struct rtnet_device *rtdev;
struct rtskb *rtskb;
int ret = 0;
if (flags & MSG_OOB) /* Mirror BSD error message compatibility */
return -EOPNOTSUPP;
/* a lot of sanity checks */
if ((flags & ~MSG_DONTWAIT) ||
(sll == NULL) || (msg->msg_namelen != sizeof(struct sockaddr_ll)) ||
((sll->sll_family != AF_PACKET) && (sll->sll_family != AF_UNSPEC)) ||
(sll->sll_ifindex <= 0))
return -EINVAL;
if ((rtdev = rtdev_get_by_index(sll->sll_ifindex)) == NULL)
return -ENODEV;
rtskb = alloc_rtskb(rtdev->hard_header_len + len, &sock->skb_pool);
if (rtskb == NULL) {
ret = -ENOBUFS;
goto out;
}
if ((len < 0) || (len > rtdev->mtu)) {
ret = -EMSGSIZE;
goto err;
}
if (sll->sll_halen != rtdev->addr_len) {
ret = -EINVAL;
goto err;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
rt_memcpy_fromkerneliovec(rtskb_put(rtskb, len), msg->msg_iov, len);
rtskb->rtdev = rtdev;
rtskb->priority = sock->priority;
if (rtdev->hard_header) {
ret = rtdev->hard_header(rtskb, rtdev, ntohs(sll->sll_protocol),
sll->sll_addr, rtdev->dev_addr, rtskb->len);
if (ret < 0)
goto err;
}
if ((rtdev->flags & IFF_UP) != 0) {
if (rtdev_xmit(rtskb) == 0)
ret = len;
else
ret = -EAGAIN;
} else {
ret = -ENETDOWN;
goto err;
}
out:
rtdev_dereference(rtdev);
return ret;
err:
kfree_rtskb(rtskb);
rtdev_dereference(rtdev);
return ret;
}
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);
}
/* ************************************************************************
* This functions runs in rtai context.
* It is called from rtnetproxy_user_srq whenever there is frame to sent out
* Copy the standard linux sk_buff buffer to a rtnet buffer and send it out
* using rtnet functions.
* ************************************************************************ */
static inline void send_data_out(struct sk_buff *skb)
{
struct rtskb *rtskb;
struct dest_route rt;
struct skb_data_format
{
struct ethhdr ethhdr;
char reserved[12]; /* Ugly but it works... All the not-interesting header bytes */
u32 ip_src;
u32 ip_dst;
} __attribute__ ((packed)); /* Important to have this structure packed!
* It represents the ethernet frame on the line and
* thus no spaces are allowed! */
struct skb_data_format *pData;
int rc;
/* Copy the data from the standard sk_buff to the realtime sk_buff:
* Both have the same length. */
rtskb = alloc_rtskb(skb->len, &rtskb_pool);
if (NULL == rtskb) {
return;
}
memcpy(rtskb->data, skb->data, skb->len);
rtskb->len = skb->len;
pData = (struct skb_data_format*) rtskb->data;
/* Determine the device to use: Only ip routing is used here.
* Non-ip protocols are not supported... */
rc = rt_ip_route_output(&rt, pData->ip_dst);
if (rc == 0)
{
struct rtnet_device *rtdev = rt.rtdev;
/* check if IP source address fits */
if (rtdev->local_ip != pData->ip_src) {
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
return;
}
rtskb->rtdev = rtdev;
/* Fill in the ethernet headers: There is already space for the header
* but they contain zeros only => Fill it */
memcpy(pData->ethhdr.h_source, rtdev->dev_addr, rtdev->addr_len);
memcpy(pData->ethhdr.h_dest, rt.dev_addr, rtdev->addr_len);
/* Call the actual transmit function */
rtdev_xmit_proxy(rtskb);
/* The rtskb is freed somewhere deep in the driver...
* No need to do it here. */
rtdev_dereference(rtdev);
}
else
{
/* Routing failed => Free rtskb here... */
kfree_rtskb(rtskb);
}
}
