/* * This file implements handling of meesagges send by drivers */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "proto.h" #include "driver.h" #if 0 #define debug_drv_print(str, ...) printf("LWIP %s:%d : " str "\n", \ __func__, __LINE__, ##__VA_ARGS__) #else #define debug_drv_print(...) debug_print(__VA_ARGS__) #endif #define RAW_BUF_SIZE (32 << 10) static struct nic devices[MAX_DEVS]; static ip_addr_t ip_addr_none = { IPADDR_NONE }; extern endpoint_t lwip_ep; void nic_assign_driver(const char * dev_type, unsigned int dev_num, const char * driver_name, unsigned int instance, int is_default) { struct nic * nic; if (strcmp(dev_type, "eth") != 0) { printf("LWIP : Cannot handle other than ethernet devices, " "ignoring '%s%d'\n", dev_type, dev_num); return; } nic = &devices[dev_num]; snprintf(nic->name, NIC_NAME_LEN, "%s%d", dev_type, dev_num); nic->name[NIC_NAME_LEN - 1] = '\0'; snprintf(nic->drv_name, DRV_NAME_LEN, "%s_%d", driver_name, instance); nic->drv_name[DRV_NAME_LEN - 1] = '\0'; nic->is_default = is_default; nic->netif.name[0] = 'e'; nic->netif.name[1] = 't'; nic->netif.num = dev_num; debug_print("/dev/%s driven by %s default = %d", nic->name, nic->drv_name, is_default); } static struct nic * lookup_nic_by_drv_ep(endpoint_t ep) { int i; for (i = 0; i < MAX_DEVS; i++) { if (devices[i].drv_ep == ep) return &devices[i]; } return NULL; } static struct nic * lookup_nic_by_drv_name(const char * name) { int i; for (i = 0; i < MAX_DEVS; i++) { if (strcmp(devices[i].drv_name, name) == 0) return &devices[i]; } return NULL; } static struct nic * lookup_nic_default(void) { int i; for (i = 0; i < MAX_DEVS; i++) { if (devices[i].is_default) return &devices[i]; } return NULL; } void nic_init_all(void) { int i; unsigned int g; for (i = 0; i < MAX_DEVS; i++) { devices[i].drv_ep = NONE; devices[i].is_default = 0; if (cpf_getgrants(&devices[i].rx_iogrant, 1) != 1) panic("Cannot initialize grants"); if (cpf_getgrants(&devices[i].rx_iovec[0].iov_grant, 1) != 1) panic("Cannot initialize grants"); if (cpf_getgrants(&devices[i].tx_iogrant, 1) != 1) panic("Cannot initialize grants"); for (g = 0; g < TX_IOVEC_NUM; g++) { cp_grant_id_t * gid = &devices[i].tx_iovec[g].iov_grant; if (cpf_getgrants(gid, 1) != 1) panic("Cannot initialize grants"); } devices[i].raw_socket = NULL; } } static void driver_setup_read(struct nic * nic) { message m; debug_print("device /dev/%s", nic->name); //assert(nic->rx_pbuf == NULL); if (!(nic->rx_pbuf == NULL)) { panic("device /dev/%s rx_pbuf %p", nic->name, nic->rx_pbuf); } if (!(nic->rx_pbuf = pbuf_alloc(PBUF_RAW, ETH_MAX_PACK_SIZE + ETH_CRC_SIZE, PBUF_RAM))) panic("Cannot allocate rx pbuf"); if (cpf_setgrant_direct(nic->rx_iovec[0].iov_grant, nic->drv_ep, (vir_bytes) nic->rx_pbuf->payload, nic->rx_pbuf->len, CPF_WRITE) != OK) panic("Failed to set grant"); nic->rx_iovec[0].iov_size = nic->rx_pbuf->len; m.m_type = DL_READV_S; m.DL_COUNT = 1; m.DL_GRANT = nic->rx_iogrant; if (asynsend(nic->drv_ep, &m) != OK) panic("asynsend to the driver failed!"); } static void nic_up(struct nic * nic, message * m) { memcpy(nic->netif.hwaddr, m->DL_HWADDR, NETIF_MAX_HWADDR_LEN); debug_print("device %s is up MAC : %02x:%02x:%02x:%02x:%02x:%02x", nic->name, nic->netif.hwaddr[0], nic->netif.hwaddr[1], nic->netif.hwaddr[2], nic->netif.hwaddr[3], nic->netif.hwaddr[4], nic->netif.hwaddr[5]); driver_setup_read(nic); netif_set_link_up(&nic->netif); netif_set_up(&nic->netif); } int driver_tx(struct nic * nic) { struct packet_q * pkt; unsigned int len; message m; int err; debug_print("device /dev/%s", nic->name); assert(nic->tx_buffer); pkt = driver_tx_head(nic); if (pkt == NULL) { debug_print("no packets enqueued"); return 0; } assert(pkt->buf_len <= nic->max_pkt_sz); if ((len = pkt->buf_len) < nic->min_pkt_sz) len = nic->min_pkt_sz; err = cpf_setgrant_direct(nic->tx_iovec[0].iov_grant, nic->drv_ep, (vir_bytes) pkt->buf, len, CPF_READ); debug_print("packet len %d", len); if (err != OK) panic("Failed to set grant"); nic->tx_iovec[0].iov_size = len; if (cpf_setgrant_direct(nic->tx_iogrant, nic->drv_ep, (vir_bytes) &nic->tx_iovec, sizeof(iovec_s_t), CPF_READ) != OK) panic("Failed to set grant"); m.m_type = DL_WRITEV_S; m.DL_COUNT = 1; m.DL_GRANT = nic->tx_iogrant; if (asynsend(nic->drv_ep, &m) != OK) panic("asynsend to the driver failed!"); nic->state = DRV_SENDING; debug_print("packet sent to driver"); return 1; } static void nic_pkt_sent(struct nic * nic) { debug_print("device /dev/%s", nic->name); assert(nic->state != DRV_IDLE); /* packet has been sent, we are not intereted anymore */ driver_tx_dequeue(nic); /* * Try to transmit the next packet. Failure means that no packet is * enqueued and thus the device is entering idle state */ if (!driver_tx(nic)) nic->state = DRV_IDLE; } __unused static void print_pkt(unsigned char * pkt, int len) { int i = 0; printf("--- PKT ---\n"); while (i < len) { int x; for (x = 0; x < 8 && i < len; x++, i++) printf("%02x ", pkt[i]); kputc(' '); for (x = 0; x < 8 && i < len; x++, i++) printf("%02x ", pkt[i]); kputc('\n'); } printf("--- PKT END ---\n"); } static int raw_receive(message * m, struct pbuf *pbuf) { struct pbuf * p; unsigned int rem_len = m->COUNT; unsigned int written = 0; int err; debug_print("user buffer size : %d\n", rem_len); for (p = pbuf; p && rem_len; p = p->next) { size_t cp_len; cp_len = (rem_len < p->len) ? rem_len : p->len; err = copy_to_user(m->m_source, p->payload, cp_len, (cp_grant_id_t) m->IO_GRANT, written); if (err != OK) return err; written += cp_len; rem_len -= cp_len; } debug_print("copied %d bytes\n", written); return written; } int raw_socket_input(struct pbuf * pbuf, struct nic * nic) { struct socket * sock; struct pbuf * pbuf_new; if ((sock = nic->raw_socket) == NULL) return 0; debug_print("socket num : %ld", get_sock_num(sock)); if (sock->flags & SOCK_FLG_OP_PENDING) { int ret; /* we are resuming a suspended operation */ ret = raw_receive(&sock->mess, pbuf); if (ret > 0) { sock_reply(sock, ret); sock->flags &= ~SOCK_FLG_OP_PENDING; return 0; } else { sock_reply(sock, ret); sock->flags &= ~SOCK_FLG_OP_PENDING; } } /* Do not enqueue more data than allowed */ if (sock->recv_data_size > RAW_BUF_SIZE) { return 0; } /* * nobody is waiting for the data or an error occured above, we enqueue * the packet. We store a copy of this packet */ pbuf_new = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_RAM); if (pbuf_new == NULL) { debug_print("LWIP : cannot allocated new pbuf\n"); return 0; } if (pbuf_copy(pbuf_new, pbuf) != ERR_OK) { debug_print("LWIP : cannot copy pbuf\n"); return 0; } /* * If we didn't managed to enqueue the packet we report it as not * consumed */ if (sock_enqueue_data(sock, pbuf_new, pbuf_new->tot_len) != OK) { pbuf_free(pbuf_new); } return 0; } static void nic_pkt_received(struct nic * nic, unsigned int size) { assert(nic->netif.input); #if 0 print_pkt((unsigned char *) nic->rx_pbuf->payload, 64 /*nic->rx_pbuf->len */); #endif assert(nic->rx_pbuf->tot_len == nic->rx_pbuf->len); nic->rx_pbuf->tot_len = nic->rx_pbuf->len = size - ETH_CRC_SIZE; nic->netif.input(nic->rx_pbuf, &nic->netif); nic->rx_pbuf = NULL; driver_setup_read(nic); } void driver_request(message * m) { struct nic * nic; if ((nic = lookup_nic_by_drv_ep(m->m_source)) == NULL) { printf("LWIP : request from unknown driver %d\n", m->m_source); return; } switch (m->m_type) { case DL_CONF_REPLY: if (m->DL_STAT == OK) nic_up(nic, m); break; case DL_TASK_REPLY: /* if (!(m->DL_FLAGS & DL_PACK_SEND) && !(m->DL_FLAGS & DL_PACK_RECV)) { printf("void reply from driver\n"); break; } */ if (m->DL_FLAGS & DL_PACK_SEND) nic_pkt_sent(nic); if (m->DL_FLAGS & DL_PACK_RECV) nic_pkt_received(nic, m->DL_COUNT); break; case DL_STAT_REPLY: break; default: printf("LWIP : unexpected request %d from driver %d\n", m->m_type, m->m_source); } } void driver_up(const char * label, endpoint_t ep) { struct nic * nic; nic = lookup_nic_by_drv_name(label); if (nic) { debug_print("LWIP : driver '%s' / %d is up for /dev/%s\n", label, ep, nic->name); nic->drv_ep = ep; } else { printf("LWIP : WARNING unexpected driver '%s' up event\n", label); return; } nic->state = DRV_IDLE; /* * FIXME * * We set the initial ip to 0.0.0.0 to make dhcpd broadcasing work * at the very begining. dhcp should use raw socket but it is a little * tricy in the current dhcp implementation */ if (!netif_add(&nic->netif, (ip_addr_t *) &ip_addr_any, &ip_addr_none, &ip_addr_none, nic, ethernetif_init, ethernet_input)) { printf("LWIP : failed to add device /dev/%s\n", nic->name); nic->drv_ep = NONE; } if (nic->is_default) netif_set_default(&nic->netif); /* FIXME we support ethernet only, 2048 is safe */ nic->tx_buffer = debug_malloc(2048); if (nic->tx_buffer == NULL) panic("Cannot allocate tx_buffer"); /* When driver restarts, the rx_pbuf is likely ready to receive data * from its previous instance. We free the buffer here, nobody depends * on it. A new one is allocated when we send a new read request to the * driver. */ if (nic->rx_pbuf) { pbuf_free(nic->rx_pbuf); nic->rx_pbuf = NULL; } /* prepare the RX grant once and forever */ if (cpf_setgrant_direct(nic->rx_iogrant, nic->drv_ep, (vir_bytes) &nic->rx_iovec, 1 * sizeof(iovec_s_t), CPF_READ) != OK) panic("Failed to set grant"); } static void raw_recv_free(__unused void * data) { pbuf_free((struct pbuf *) data); } static void nic_op_close(struct socket * sock, __unused message * m) { struct nic * nic = (struct nic *)sock->data; debug_drv_print("socket %d", get_sock_num(sock)); sock_dequeue_data_all(sock, raw_recv_free); sock->ops = NULL; if (nic->raw_socket == sock) { nic->raw_socket = NULL; debug_drv_print("no active raw sock at %s", nic->name); } sock_reply_close(sock, OK); } static void nic_ioctl_set_conf(__unused struct socket * sock, struct nic * nic, message * m) { nwio_ipconf_t ipconf; int err; err = copy_from_user(m->m_source, &ipconf, sizeof(ipconf), (cp_grant_id_t) m->IO_GRANT, 0); if (err != OK) send_reply(m, err); if (ipconf.nwic_flags & NWIC_IPADDR_SET) netif_set_ipaddr(&nic->netif, (ip_addr_t *)&ipconf.nwic_ipaddr); if (ipconf.nwic_flags & NWIC_NETMASK_SET) netif_set_netmask(&nic->netif, (ip_addr_t *)&ipconf.nwic_netmask); nic->flags = ipconf.nwic_flags; if (nic->flags & NWEO_EN_BROAD) nic->netif.flags |= NETIF_FLAG_BROADCAST; send_reply(m, OK); } static void nic_ioctl_get_conf(__unused struct socket * sock, struct nic * nic, message * m) { nwio_ipconf_t ipconf; int err; ipconf.nwic_flags = nic->flags; ipconf.nwic_ipaddr = nic->netif.ip_addr.addr; ipconf.nwic_netmask = nic->netif.netmask.addr; ipconf.nwic_mtu = nic->netif.mtu; err = copy_to_user(m->m_source, &ipconf, sizeof(ipconf), (cp_grant_id_t) m->IO_GRANT, 0); if (err != OK) send_reply(m, err); send_reply(m, OK); } static void nic_ioctl_set_gateway(__unused struct socket * sock, struct nic * nic, message * m) { nwio_route_t route; int err; err = copy_from_user(m->m_source, &route, sizeof(route), (cp_grant_id_t) m->IO_GRANT, 0); if (err != OK) send_reply(m, err); netif_set_gw(&nic->netif, (ip_addr_t *)&route.nwr_gateway); send_reply(m, OK); } static void nic_ioctl_get_ethstat(__unused struct socket * sock, struct nic * nic, message * m) { int err; nwio_ethstat_t ethstat; debug_drv_print("device /dev/%s", nic->name); /* * The device is not up yet, there is nothing to report or it is not * an ethernet device */ if (!nic->netif.flags & NETIF_FLAG_UP || !(nic->netif.flags & (NETIF_FLAG_ETHERNET | NETIF_FLAG_ETHARP))) { printf("LWIP no such device FUCK\n"); send_reply(m, ENODEV); return; } memset(ðstat, 0, sizeof(ethstat)); memcpy(ðstat.nwes_addr, nic->netif.hwaddr, 6); err = copy_to_user(m->m_source, ðstat, sizeof(ethstat), (cp_grant_id_t) m->IO_GRANT, 0); if (err != OK) send_reply(m, err); send_reply(m, OK); } static void nic_ioctl_set_ethopt(struct socket * sock, struct nic * nic, message * m) { int err; nwio_ethopt_t ethopt; assert(nic); if (!sock) { send_reply(m, EINVAL); return; } debug_drv_print("device /dev/%s", nic->name); /* * The device is not up yet, there is nothing to report or it is not * an ethernet device */ if (!nic->netif.flags & NETIF_FLAG_UP || !(nic->netif.flags & (NETIF_FLAG_ETHERNET | NETIF_FLAG_ETHARP))) { send_reply(m, ENODEV); return; } err = copy_from_user(m->m_source, ðopt, sizeof(ethopt), (cp_grant_id_t) m->IO_GRANT, 0); if (err != OK) send_reply(m, err); /* we want to get data from this sock */ if (ethopt.nweo_flags & NWEO_COPY) { if (nic->raw_socket) { send_reply(m, EBUSY); return; } nic->raw_socket = sock; debug_drv_print("active raw sock %d at %s", get_sock_num(sock), nic->name); } send_reply(m, OK); } static void nic_do_ioctl(struct socket * sock, struct nic * nic, message * m) { debug_print("device /dev/%s req %c %d %d", nic->name, (m->REQUEST >> 8) & 0xff, m->REQUEST & 0xff, (m->REQUEST >> 16) & _IOCPARM_MASK); debug_drv_print("socket %d", sock ? get_sock_num(sock) : -1); switch (m->REQUEST) { case NWIOSIPCONF: nic_ioctl_set_conf(sock, nic, m); break; case NWIOGIPCONF: nic_ioctl_get_conf(sock, nic, m); break; case NWIOSIPOROUTE: nic_ioctl_set_gateway(sock, nic, m); break; case NWIOGETHSTAT: nic_ioctl_get_ethstat(sock, nic, m); break; case NWIOSETHOPT: nic_ioctl_set_ethopt(sock, nic, m); break; default: send_reply(m, EBADIOCTL); return; } } void nic_default_ioctl(message *m) { struct nic * nic = lookup_nic_default(); if (nic == NULL) { debug_print("No default nic, reporting error"); send_reply(m, EBADIOCTL); return; } nic_do_ioctl(NULL, nic, m); } static void nic_op_ioctl(struct socket * sock, message * m, __unused int blk) { nic_do_ioctl(sock, (struct nic *)sock->data, m); } static void nic_op_read(struct socket * sock, message * m, int blk) { debug_drv_print("sock num %d", get_sock_num(sock)); if (sock->recv_head) { /* data available receive immeditely */ struct pbuf * pbuf; int ret; pbuf = sock->recv_head->data; ret = raw_receive(m, pbuf); if (ret > 0) { sock_dequeue_data(sock); sock->recv_data_size -= pbuf->tot_len; pbuf_free(pbuf); } sock_reply(sock, ret); } else if (!blk) send_reply(m, EAGAIN); else { /* store the message so we know how to reply */ sock->mess = *m; /* operation is being processes */ sock->flags |= SOCK_FLG_OP_PENDING; debug_print("no data to read, suspending"); } } static void nic_op_write(struct socket * sock, message * m, __unused int blk) { int ret; struct pbuf * pbuf; struct nic * nic = (struct nic *)sock->data; assert(nic); debug_print("device %s data size %d", nic->name, get_sock_num(sock), m->COUNT); pbuf = pbuf_alloc(PBUF_RAW, m->COUNT, PBUF_RAM); if (!pbuf) { ret = ENOMEM; goto write_err; } if ((ret = copy_from_user(m->m_source, pbuf->payload, m->COUNT, (cp_grant_id_t) m->IO_GRANT, 0)) != OK) { pbuf_free(pbuf); goto write_err; } if ((ret = nic->netif.linkoutput(&nic->netif, pbuf) != ERR_OK)) { debug_print("raw linkoutput failed %d", ret); ret = EIO; } else ret = m->COUNT; pbuf_free(pbuf); write_err: sock_reply(sock, ret); } static struct sock_ops nic_ops = { .write = nic_op_write, .read = nic_op_read, .close = nic_op_close, .ioctl = nic_op_ioctl, .select = generic_op_select, .select_reply = generic_op_select_reply }; void nic_open(message *m) { struct socket * sock; debug_print("device %d", m->DEVICE); if (m->DEVICE > MAX_DEVS || devices[m->DEVICE].drv_ep == NONE) { send_reply_open(m, ENODEV); return; } sock = get_unused_sock(); if (sock == NULL) { send_reply(m, ENODEV); return; } if (sock->ops != NULL) { send_reply(m, EBUSY); return; } sock->ops = &nic_ops; sock->select_ep = NONE; sock->recv_data_size = 0; sock->data = &devices[m->DEVICE]; send_reply_open(m, get_sock_num(sock)); } static int driver_pkt_enqueue(struct packet_q ** head, struct packet_q ** tail, struct pbuf * pbuf) { struct packet_q * pkt; char * b; pkt = (struct packet_q *) malloc(sizeof(struct packet_q) + pbuf->tot_len); if (!pkt) return ENOMEM; pkt->next = NULL; pkt->buf_len = pbuf->tot_len; for (b = pkt->buf; pbuf; pbuf = pbuf->next) { memcpy(b, pbuf->payload, pbuf->len); b += pbuf->len; } if (*head == NULL) *head = *tail = pkt; else { (*tail)->next = pkt; *tail = pkt; } return OK; } int driver_tx_enqueue(struct nic * nic, struct pbuf * pbuf) { debug_print("device /dev/%s", nic->name); return driver_pkt_enqueue(&nic->tx_head, &nic->tx_tail, pbuf); } static void driver_pkt_dequeue(struct packet_q ** head, struct packet_q ** tail) { struct packet_q * pkt; /* we always dequeue only if there is something to dequeue */ assert(*head); pkt = *head; if ((*head = pkt->next) == NULL) *tail = NULL; debug_free(pkt); } void driver_tx_dequeue(struct nic * nic) { debug_print("device /dev/%s", nic->name); driver_pkt_dequeue(&nic->tx_head, &nic->tx_tail); } struct packet_q * driver_tx_head(struct nic * nic) { debug_print("device /dev/%s", nic->name); if (!nic->tx_head) return NULL; return nic->tx_head; }