servers/lwip/driver.c

   1 /*
   2  * This file implements handling of meesagges send by drivers
   3  */
   4
   5 #include <stdio.h>
   6 #include <stdlib.h>
   7
   8 #include <minix/ipc.h>
   9 #include <minix/com.h>
  10 #include <minix/sysutil.h>
  11 #include <minix/safecopies.h>
  12 #include <minix/netsock.h>
  13
  14 #include <sys/ioc_net.h>
  15 #include <net/gen/in.h>
  16 #include <net/gen/ip_io.h>
  17 #include <net/gen/route.h>
  18 #include <net/gen/ether.h>
  19 #include <net/gen/eth_io.h>
  20
  21 #include <lwip/pbuf.h>
  22 #include <lwip/netif.h>
  23 #include <netif/etharp.h>
  24
  25 #include "proto.h"
  26 #include "driver.h"
  27
  28 #if 0
  29 #define debug_drv_print(str, ...) printf("LWIP %s:%d : " str "\n", \
  30                 __func__, __LINE__, ##__VA_ARGS__)
  31 #else
  32 #define debug_drv_print(...) debug_print(__VA_ARGS__)
  33 #endif
  34
  35 #define RAW_BUF_SIZE    (32 << 10)
  36
  37 static struct nic devices[MAX_DEVS];
  38
  39 static ip_addr_t ip_addr_none = { IPADDR_NONE };
  40 extern endpoint_t lwip_ep;
  41
  42 void nic_assign_driver(const char * dev_type,
  43                         unsigned dev_num,
  44                         const char * driver_name,
  45                         unsigned instance,
  46                         int is_default)
  47 {
  48         struct nic * nic;
  49
  50         if (strcmp(dev_type, "eth") != 0) {
  51                 printf("LWIP : Cannot handle other than ethernet devices, "
  52                                 "ignoring '%s%d'\n", dev_type, dev_num);
  53                 return;
  54         }
  55
  56         nic = &devices[dev_num];
  57         snprintf(nic->name, NIC_NAME_LEN, "%s%d", dev_type, dev_num);
  58         nic->name[NIC_NAME_LEN - 1] = '\0';
  59         snprintf(nic->drv_name, DRV_NAME_LEN, "%s_%d", driver_name, instance);
  60         nic->drv_name[DRV_NAME_LEN - 1] = '\0';
  61         nic->is_default = is_default;
  62         nic->netif.name[0] = 'e';
  63         nic->netif.name[1] = 't';
  64         nic->netif.num = dev_num;
  65
  66         debug_print("/dev/%s driven by %s default = %d",
  67                         nic->name, nic->drv_name, is_default);
  68 }
  69
  70 static struct nic * lookup_nic_by_drv_ep(endpoint_t ep)
  71 {
  72         int i;
  73
  74         for (i = 0; i < MAX_DEVS; i++) {
  75                 if (devices[i].drv_ep == ep)
  76                         return &devices[i];
  77         }
  78
  79         return NULL;
  80 }
  81
  82 static struct nic * lookup_nic_by_drv_name(const char * name)
  83 {
  84         int i;
  85
  86         for (i = 0; i < MAX_DEVS; i++) {
  87                 if (strcmp(devices[i].drv_name, name) == 0)
  88                         return &devices[i];
  89         }
  90
  91         return NULL;
  92 }
  93
  94 static struct nic * lookup_nic_default(void)
  95 {
  96         int i;
  97
  98         for (i = 0; i < MAX_DEVS; i++) {
  99                 if (devices[i].is_default)
 100                         return &devices[i];
 101         }
 102
 103         return NULL;
 104 }
 105
 106 void nic_init_all(void)
 107 {
 108         int i;
 109         unsigned g;
 110
 111         for (i = 0; i < MAX_DEVS; i++) {
 112                 devices[i].drv_ep = NONE;
 113                 devices[i].is_default = 0;
 114
 115                 if (cpf_getgrants(&devices[i].rx_iogrant, 1) != 1)
 116                         panic("Cannot initialize grants");
 117                 if (cpf_getgrants(&devices[i].rx_iovec[0].iov_grant, 1) != 1)
 118                         panic("Cannot initialize grants");
 119                 if (cpf_getgrants(&devices[i].tx_iogrant, 1) != 1)
 120                         panic("Cannot initialize grants");
 121                 for (g = 0; g < TX_IOVEC_NUM; g++) {
 122                         cp_grant_id_t * gid = &devices[i].tx_iovec[g].iov_grant;
 123                         if (cpf_getgrants(gid, 1) != 1)
 124                                 panic("Cannot initialize grants");
 125                 }
 126                 devices[i].raw_socket = NULL;
 127         }
 128 }
 129
 130 static void driver_setup_read(struct nic * nic)
 131 {
 132         message m;
 133
 134         debug_print("device /dev/%s", nic->name);
 135         //assert(nic->rx_pbuf == NULL);
 136         if (!(nic->rx_pbuf == NULL)) {
 137                 panic("device /dev/%s rx_pbuf %p", nic->name, nic->rx_pbuf);
 138         }
 139
 140         if (!(nic->rx_pbuf = pbuf_alloc(PBUF_RAW, ETH_MAX_PACK_SIZE + ETH_CRC_SIZE, PBUF_RAM)))
 141                 panic("Cannot allocate rx pbuf");
 142
 143         if (cpf_setgrant_direct(nic->rx_iovec[0].iov_grant,
 144                                 nic->drv_ep, (vir_bytes) nic->rx_pbuf->payload,
 145                                 nic->rx_pbuf->len, CPF_WRITE) != OK)
 146                 panic("Failed to set grant");
 147         nic->rx_iovec[0].iov_size = nic->rx_pbuf->len;
 148
 149         m.m_type = DL_READV_S;
 150         m.DL_COUNT = 1;
 151         m.DL_GRANT = nic->rx_iogrant;
 152
 153         if (asynsend(nic->drv_ep, &m) != OK)
 154                 panic("asynsend to the driver failed!");
 155 }
 156
 157 static void nic_up(struct nic * nic, message * m)
 158 {
 159         memcpy(nic->netif.hwaddr, m->DL_HWADDR, NETIF_MAX_HWADDR_LEN);
 160
 161         debug_print("device %s is up MAC : %02x:%02x:%02x:%02x:%02x:%02x",
 162                         nic->name,
 163                         nic->netif.hwaddr[0],
 164                         nic->netif.hwaddr[1],
 165                         nic->netif.hwaddr[2],
 166                         nic->netif.hwaddr[3],
 167                         nic->netif.hwaddr[4],
 168                         nic->netif.hwaddr[5]);
 169
 170         driver_setup_read(nic);
 171
 172         netif_set_link_up(&nic->netif);
 173         netif_set_up(&nic->netif);
 174 }
 175
 176 int driver_tx(struct nic * nic)
 177 {
 178         struct packet_q * pkt;
 179         unsigned len;
 180         message m;
 181
 182         int err;
 183
 184         debug_print("device /dev/%s", nic->name);
 185         assert(nic->tx_buffer);
 186
 187         pkt = driver_tx_head(nic);
 188         if (pkt == NULL) {
 189                 debug_print("no packets enqueued");
 190                 return 0;
 191         }
 192
 193         assert(pkt->buf_len <= nic->max_pkt_sz);
 194
 195         if ((len = pkt->buf_len) < nic->min_pkt_sz)
 196                 len = nic->min_pkt_sz;
 197         err = cpf_setgrant_direct(nic->tx_iovec[0].iov_grant,
 198                         nic->drv_ep, (vir_bytes) pkt->buf,
 199                         len, CPF_READ);
 200         debug_print("packet len %d", len);
 201         if (err != OK)
 202                 panic("Failed to set grant");
 203         nic->tx_iovec[0].iov_size = len;
 204
 205         if (cpf_setgrant_direct(nic->tx_iogrant, nic->drv_ep,
 206                         (vir_bytes) &nic->tx_iovec,
 207                         sizeof(iovec_s_t), CPF_READ) != OK)
 208                 panic("Failed to set grant");
 209
 210         m.m_type = DL_WRITEV_S;
 211         m.DL_COUNT = 1;
 212         m.DL_GRANT = nic->tx_iogrant;
 213
 214         if (asynsend(nic->drv_ep, &m) != OK)
 215                 panic("asynsend to the driver failed!");
 216         nic->state = DRV_SENDING;
 217
 218         debug_print("packet sent to driver");
 219
 220         return 1;
 221 }
 222
 223 static void nic_pkt_sent(struct nic * nic)
 224 {
 225         debug_print("device /dev/%s", nic->name);
 226         assert(nic->state != DRV_IDLE);
 227
 228         /* packet has been sent, we are not intereted anymore */
 229         driver_tx_dequeue(nic);
 230         /*
 231          * Try to transmit the next packet. Failure means that no packet is
 232          * enqueued and thus the device is entering idle state
 233          */
 234         if (!driver_tx(nic))
 235                 nic->state = DRV_IDLE;
 236 }
 237
 238 __unused static void print_pkt(unsigned char * pkt, int len)
 239 {
 240         int i = 0;
 241
 242         printf("--- PKT ---\n");
 243
 244         while (i < len) {
 245                 int x;
 246
 247                 for (x = 0; x < 8 && i < len; x++, i++)
 248                         printf("%02x ", pkt[i]);
 249
 250                 kputc(' ');
 251
 252                 for (x = 0; x < 8 && i < len; x++, i++)
 253                         printf("%02x ", pkt[i]);
 254
 255                 kputc('\n');
 256         }
 257
 258         printf("--- PKT END ---\n");
 259 }
 260
 261 static int raw_receive(message * m,
 262                         struct pbuf *pbuf)
 263 {
 264         struct pbuf * p;
 265         unsigned rem_len = m->COUNT;
 266         unsigned written = 0;
 267         int err;
 268
 269         debug_print("user buffer size : %d\n", rem_len);
 270
 271         for (p = pbuf; p && rem_len; p = p->next) {
 272                 size_t cp_len;
 273
 274                 cp_len = (rem_len < p->len) ? rem_len : p->len;
 275                 err = copy_to_user(m->m_source, p->payload, cp_len,
 276                                 (cp_grant_id_t) m->IO_GRANT,
 277                                 written);
 278
 279                 if (err != OK)
 280                         return err;
 281
 282                 written += cp_len;
 283                 rem_len -= cp_len;
 284         }
 285
 286         debug_print("copied %d bytes\n", written);
 287         return written;
 288 }
 289
 290 int raw_socket_input(struct pbuf * pbuf, struct nic * nic)
 291 {
 292         struct socket * sock;
 293         struct pbuf * pbuf_new;
 294
 295         if ((sock = nic->raw_socket) == NULL)
 296                 return 0;
 297
 298         debug_print("socket num : %ld", get_sock_num(sock));
 299
 300         if (sock->flags & SOCK_FLG_OP_PENDING) {
 301                 int ret;
 302                 /* we are resuming a suspended operation */
 303                 ret = raw_receive(&sock->mess, pbuf);
 304
 305                 if (ret > 0) {
 306                         sock_reply(sock, ret);
 307                         sock->flags &= ~SOCK_FLG_OP_PENDING;
 308                         return 0;
 309                 } else {
 310                         sock_reply(sock, ret);
 311                         sock->flags &= ~SOCK_FLG_OP_PENDING;
 312                 }
 313         }
 314
 315         /* Do not enqueue more data than allowed */
 316         if (sock->recv_data_size > RAW_BUF_SIZE) {
 317                 return 0;
 318         }
 319
 320         /*
 321          * nobody is waiting for the data or an error occured above, we enqueue
 322          * the packet. We store a copy of this packet
 323          */
 324         pbuf_new = pbuf_alloc(PBUF_RAW, pbuf->tot_len, PBUF_RAM);
 325         if (pbuf_new == NULL) {
 326                 debug_print("LWIP : cannot allocated new pbuf\n");
 327                 return 0;
 328         }
 329
 330         if (pbuf_copy(pbuf_new, pbuf) != ERR_OK) {
 331                 debug_print("LWIP : cannot copy pbuf\n");
 332                 return 0;
 333         }
 334
 335         /*
 336          * If we didn't managed to enqueue the packet we report it as not
 337          * consumed
 338          */
 339         if (sock_enqueue_data(sock, pbuf_new, pbuf_new->tot_len) != OK) {
 340                 pbuf_free(pbuf_new);
 341         }
 342
 343         return 0;
 344 }
 345
 346 static void nic_pkt_received(struct nic * nic, unsigned size)
 347 {
 348         assert(nic->netif.input);
 349
 350 #if 0
 351         print_pkt((unsigned char *) nic->rx_pbuf->payload, 64 /*nic->rx_pbuf->len */);
 352 #endif
 353
 354         assert(nic->rx_pbuf->tot_len == nic->rx_pbuf->len);
 355         nic->rx_pbuf->tot_len = nic->rx_pbuf->len = size - ETH_CRC_SIZE;
 356
 357         nic->netif.input(nic->rx_pbuf, &nic->netif);
 358         nic->rx_pbuf = NULL;
 359         driver_setup_read(nic);
 360 }
 361
 362 void driver_request(message * m)
 363 {
 364         struct nic * nic;
 365
 366         if ((nic = lookup_nic_by_drv_ep(m->m_source)) == NULL) {
 367                 printf("LWIP : request from unknown driver %d\n", m->m_source);
 368                 return;
 369         }
 370
 371         switch (m->m_type) {
 372         case DL_CONF_REPLY:
 373                 if (m->DL_STAT == OK)
 374                         nic_up(nic, m);
 375                 break;
 376         case DL_TASK_REPLY:
 377                 /*
 378                 if (!(m->DL_FLAGS & DL_PACK_SEND) && !(m->DL_FLAGS & DL_PACK_RECV)) {
 379                         printf("void reply from driver\n");
 380                         break;
 381                 }
 382                 */
 383                 if (m->DL_FLAGS & DL_PACK_SEND)
 384                         nic_pkt_sent(nic);
 385                 if (m->DL_FLAGS & DL_PACK_RECV)
 386                         nic_pkt_received(nic, m->DL_COUNT);
 387                 break;
 388         case DL_STAT_REPLY:
 389                 break;
 390         default:
 391                 printf("LWIP : unexpected request %d from driver %d\n",
 392                                                 m->m_type, m->m_source);
 393         }
 394 }
 395
 396 void driver_up(const char * label, endpoint_t ep)
 397 {
 398         struct nic * nic;
 399
 400         nic = lookup_nic_by_drv_name(label);
 401
 402         if (nic) {
 403                 debug_print("LWIP : driver '%s' / %d is up for /dev/%s\n",
 404                                 label, ep, nic->name);
 405                 nic->drv_ep = ep;
 406         } else
 407                 printf("LWIP : WARNING unexpected driver '%s' up event\n",
 408                                                                 label);
 409
 410         nic->state = DRV_IDLE;
 411
 412         /*
 413          * FIXME
 414          *
 415          * We set the initial ip to 0.0.0.0 to make dhcpd broadcasing work
 416          * at the very begining. dhcp should use raw socket but it is a little
 417          * tricy in the current dhcp implementation
 418          */
 419         if (!netif_add(&nic->netif, (ip_addr_t *) &ip_addr_any, &ip_addr_none,
 420                         &ip_addr_none, nic, ethernetif_init, ethernet_input)) {
 421                 printf("LWIP : failed to add device /dev/%s\n", nic->name);
 422                 nic->drv_ep = NONE;
 423         }
 424         if (nic->is_default)
 425                 netif_set_default(&nic->netif);
 426
 427         /* FIXME we support ethernet only, 2048 is safe */
 428         nic->tx_buffer = debug_malloc(2048);
 429         if (nic->tx_buffer == NULL)
 430                 panic("Cannot allocate tx_buffer");
 431
 432         /* prepare the RX grant once and forever */
 433         if (cpf_setgrant_direct(nic->rx_iogrant,
 434                                 nic->drv_ep,
 435                                 (vir_bytes) &nic->rx_iovec,
 436                                 1 * sizeof(iovec_s_t), CPF_READ) != OK)
 437                 panic("Failed to set grant");
 438 }
 439
 440 static void raw_recv_free(__unused void * data)
 441 {
 442         pbuf_free((struct pbuf *) data);
 443 }
 444
 445 static void nic_op_close(struct socket * sock, __unused message * m)
 446 {
 447         struct nic * nic = (struct nic *)sock->data;
 448
 449         debug_drv_print("socket %d", get_sock_num(sock));
 450
 451         sock_dequeue_data_all(sock, raw_recv_free);
 452         sock->ops = NULL;
 453
 454         if (nic->raw_socket == sock) {
 455                 nic->raw_socket = NULL;
 456                 debug_drv_print("no active raw sock at %s", nic->name);
 457         }
 458
 459         sock_reply_close(sock, OK);
 460 }
 461
 462 static void nic_ioctl_set_conf(__unused struct socket * sock,
 463                                 struct nic * nic,
 464                                 message * m)
 465 {
 466         nwio_ipconf_t ipconf;
 467         int err;
 468
 469         err = copy_from_user(m->m_source, &ipconf, sizeof(ipconf),
 470                                 (cp_grant_id_t) m->IO_GRANT, 0);
 471         if (err != OK)
 472                 send_reply(m, err);
 473
 474         if (ipconf.nwic_flags & NWIC_IPADDR_SET)
 475                 netif_set_ipaddr(&nic->netif,
 476                                 (ip_addr_t *)&ipconf.nwic_ipaddr);
 477         if (ipconf.nwic_flags & NWIC_NETMASK_SET)
 478                 netif_set_netmask(&nic->netif,
 479                                 (ip_addr_t *)&ipconf.nwic_netmask);
 480         nic->flags = ipconf.nwic_flags;
 481         if (nic->flags & NWEO_EN_BROAD)
 482                 nic->netif.flags |= NETIF_FLAG_BROADCAST;
 483
 484         send_reply(m, OK);
 485 }
 486
 487 static void nic_ioctl_get_conf(__unused struct socket * sock,
 488                                 struct nic * nic,
 489                                 message * m)
 490 {
 491         nwio_ipconf_t ipconf;
 492         int err;
 493
 494         ipconf.nwic_flags = nic->flags;
 495         ipconf.nwic_ipaddr = nic->netif.ip_addr.addr;
 496         ipconf.nwic_netmask = nic->netif.netmask.addr;
 497         ipconf.nwic_mtu = nic->netif.mtu;
 498
 499         err = copy_to_user(m->m_source, &ipconf, sizeof(ipconf),
 500                                 (cp_grant_id_t) m->IO_GRANT, 0);
 501         if (err != OK)
 502                 send_reply(m, err);
 503
 504         send_reply(m, OK);
 505 }
 506
 507 static void nic_ioctl_set_gateway(__unused struct socket * sock,
 508                                 struct nic * nic,
 509                                 message * m)
 510 {
 511         nwio_route_t route;
 512         int err;
 513
 514         err = copy_from_user(m->m_source, &route, sizeof(route),
 515                                 (cp_grant_id_t) m->IO_GRANT, 0);
 516         if (err != OK)
 517                 send_reply(m, err);
 518
 519         netif_set_gw(&nic->netif, (ip_addr_t *)&route.nwr_gateway);
 520
 521         send_reply(m, OK);
 522 }
 523
 524 static void nic_ioctl_get_ethstat(__unused struct socket * sock,
 525                                 struct nic * nic,
 526                                 message * m)
 527 {
 528         int err;
 529         nwio_ethstat_t ethstat;
 530
 531         debug_drv_print("device /dev/%s", nic->name);
 532         /*
 533          * The device is not up yet, there is nothing to report or it is not
 534          * an ethernet device
 535          */
 536         if (!nic->netif.flags & NETIF_FLAG_UP ||
 537                         !(nic->netif.flags & (NETIF_FLAG_ETHERNET |
 538                                 NETIF_FLAG_ETHARP))) {
 539                 printf("LWIP no such device FUCK\n");
 540                 send_reply(m, ENODEV);
 541                 return;
 542         }
 543
 544         memset(&ethstat, 0, sizeof(ethstat));
 545         memcpy(&ethstat.nwes_addr, nic->netif.hwaddr, 6);
 546
 547         err = copy_to_user(m->m_source, &ethstat, sizeof(ethstat),
 548                                 (cp_grant_id_t) m->IO_GRANT, 0);
 549         if (err != OK)
 550                 send_reply(m, err);
 551
 552         send_reply(m, OK);
 553 }
 554
 555 static void nic_ioctl_set_ethopt(struct socket * sock,
 556                                 struct nic * nic,
 557                                 message * m)
 558 {
 559         int err;
 560         nwio_ethopt_t ethopt;
 561
 562         assert(nic);
 563
 564         if (!sock) {
 565                 send_reply(m, EINVAL);
 566                 return;
 567         }
 568
 569         debug_drv_print("device /dev/%s", nic->name);
 570         /*
 571          * The device is not up yet, there is nothing to report or it is not
 572          * an ethernet device
 573          */
 574         if (!nic->netif.flags & NETIF_FLAG_UP ||
 575                         !(nic->netif.flags & (NETIF_FLAG_ETHERNET |
 576                                 NETIF_FLAG_ETHARP))) {
 577                 send_reply(m, ENODEV);
 578                 return;
 579         }
 580
 581         err = copy_from_user(m->m_source, &ethopt, sizeof(ethopt),
 582                                 (cp_grant_id_t) m->IO_GRANT, 0);
 583         if (err != OK)
 584                 send_reply(m, err);
 585
 586         /* we want to get data from this sock */
 587         if (ethopt.nweo_flags & NWEO_COPY) {
 588                 if (nic->raw_socket) {
 589                         send_reply(m, EBUSY);
 590                         return;
 591                 }
 592
 593                 nic->raw_socket = sock;
 594                 debug_drv_print("active raw sock %d at %s",
 595                                 get_sock_num(sock), nic->name);
 596         }
 597
 598         send_reply(m, OK);
 599 }
 600
 601 static void nic_do_ioctl(struct socket * sock, struct nic * nic, message * m)
 602 {
 603         debug_print("device /dev/%s req %c %d %d",
 604                         nic->name,
 605                         (m->REQUEST >> 8) & 0xff,
 606                         m->REQUEST & 0xff,
 607                         (m->REQUEST >> 16) & _IOCPARM_MASK);
 608
 609         debug_drv_print("socket %d", sock ? get_sock_num(sock) : -1);
 610
 611         switch (m->REQUEST) {
 612         case NWIOSIPCONF:
 613                 nic_ioctl_set_conf(sock, nic, m);
 614                 break;
 615         case NWIOGIPCONF:
 616                 nic_ioctl_get_conf(sock, nic, m);
 617                 break;
 618         case NWIOSIPOROUTE:
 619                 nic_ioctl_set_gateway(sock, nic, m);
 620                 break;
 621         case NWIOGETHSTAT:
 622                 nic_ioctl_get_ethstat(sock, nic, m);
 623                 break;
 624         case NWIOSETHOPT:
 625                 nic_ioctl_set_ethopt(sock, nic, m);
 626                 break;
 627         default:
 628                 send_reply(m, EBADIOCTL);
 629                 return;
 630         }
 631 }
 632
 633 void nic_default_ioctl(message *m)
 634 {
 635         struct nic * nic = lookup_nic_default();
 636
 637         if (nic == NULL) {
 638                 debug_print("No default nic, reporting error");
 639                 send_reply(m, EBADIOCTL);
 640                 return;
 641         }
 642
 643         nic_do_ioctl(NULL, nic, m);
 644 }
 645
 646 static void nic_op_ioctl(struct socket * sock, message * m, __unused int blk)
 647 {
 648         nic_do_ioctl(sock, (struct nic *)sock->data, m);
 649 }
 650
 651 static void nic_op_read(struct socket * sock, message * m, int blk)
 652 {
 653         debug_drv_print("sock num %d", get_sock_num(sock));
 654
 655         if (sock->recv_head) {
 656                 /* data available receive immeditely */
 657
 658                 struct pbuf * pbuf;
 659                 int ret;
 660
 661                 pbuf = sock->recv_head->data;
 662
 663                 ret = raw_receive(m, pbuf);
 664
 665                 if (ret > 0) {
 666                         sock_dequeue_data(sock);
 667                         sock->recv_data_size -= pbuf->tot_len;
 668                         pbuf_free(pbuf);
 669                 }
 670                 sock_reply(sock, ret);
 671         } else if (!blk)
 672                 send_reply(m, EAGAIN);
 673         else {
 674                 /* store the message so we know how to reply */
 675                 sock->mess = *m;
 676                 /* operation is being processes */
 677                 sock->flags |= SOCK_FLG_OP_PENDING;
 678
 679                 debug_print("no data to read, suspending");
 680         }
 681 }
 682
 683 static void nic_op_write(struct socket * sock, message * m, __unused int blk)
 684 {
 685         int ret;
 686         struct pbuf * pbuf;
 687         struct nic * nic = (struct nic *)sock->data;
 688
 689         assert(nic);
 690         debug_print("device %s data size %d", nic->name,
 691                         get_sock_num(sock), m->COUNT);
 692
 693         pbuf = pbuf_alloc(PBUF_RAW, m->COUNT, PBUF_RAM);
 694         if (!pbuf) {
 695                 ret = ENOMEM;
 696                 goto write_err;
 697         }
 698
 699         if ((ret = copy_from_user(m->m_source, pbuf->payload, m->COUNT,
 700                                 (cp_grant_id_t) m->IO_GRANT, 0)) != OK) {
 701                 pbuf_free(pbuf);
 702                 goto write_err;
 703         }
 704
 705         if ((ret = nic->netif.linkoutput(&nic->netif, pbuf) != ERR_OK)) {
 706                 debug_print("raw linkoutput failed %d", ret);
 707                 ret = EIO;
 708         } else
 709                 ret = m->COUNT;
 710
 711
 712         pbuf_free(pbuf);
 713
 714 write_err:
 715         sock_reply(sock, ret);
 716 }
 717
 718 static struct sock_ops nic_ops = {
 719         .write          = nic_op_write,
 720         .read           = nic_op_read,
 721         .close          = nic_op_close,
 722         .ioctl          = nic_op_ioctl,
 723         .select         = generic_op_select,
 724         .select_reply   = generic_op_select_reply
 725 };
 726
 727 void nic_open(message *m)
 728 {
 729         struct socket * sock;
 730
 731         debug_print("device %d", m->DEVICE);
 732
 733         if (m->DEVICE > MAX_DEVS || devices[m->DEVICE].drv_ep == NONE) {
 734                 send_reply_open(m, ENODEV);
 735                 return;
 736         }
 737
 738         sock = get_unused_sock();
 739
 740         if (sock == NULL) {
 741                 send_reply(m, ENODEV);
 742                 return;
 743         }
 744         if (sock->ops != NULL) {
 745                 send_reply(m, EBUSY);
 746                 return;
 747         }
 748
 749         sock->ops = &nic_ops;
 750         sock->select_ep = NONE;
 751         sock->recv_data_size = 0;
 752         sock->data = &devices[m->DEVICE];
 753
 754         send_reply_open(m, get_sock_num(sock));
 755 }
 756
 757 static int driver_pkt_enqueue(struct packet_q ** head,
 758                                 struct packet_q ** tail,
 759                                 struct pbuf * pbuf)
 760 {
 761         struct packet_q * pkt;
 762         char * b;
 763
 764         pkt = (struct packet_q *) malloc(sizeof(struct packet_q) + pbuf->tot_len);
 765         if (!pkt)
 766                 return ENOMEM;
 767
 768         pkt->next = NULL;
 769         pkt->buf_len = pbuf->tot_len;
 770
 771         for (b = pkt->buf; pbuf; pbuf = pbuf->next) {
 772                 memcpy(b, pbuf->payload, pbuf->len);
 773                 b += pbuf->len;
 774         }
 775
 776         if (*head == NULL)
 777                 *head = *tail = pkt;
 778         else {
 779                 (*tail)->next = pkt;
 780                 *tail = pkt;
 781         }
 782
 783         return OK;
 784 }
 785
 786 int driver_tx_enqueue(struct nic * nic, struct pbuf * pbuf)
 787 {
 788         debug_print("device /dev/%s", nic->name);
 789         return driver_pkt_enqueue(&nic->tx_head, &nic->tx_tail, pbuf);
 790 }
 791
 792 static void driver_pkt_dequeue(struct packet_q ** head,
 793                                         struct packet_q ** tail)
 794 {
 795         struct packet_q * pkt;
 796
 797         /* we always dequeue only if there is something to dequeue */
 798         assert(*head);
 799
 800         pkt = *head;
 801
 802         if ((*head = pkt->next) == NULL)
 803                 *tail = NULL;
 804
 805         debug_free(pkt);
 806 }
 807
 808 void driver_tx_dequeue(struct nic * nic)
 809 {
 810         debug_print("device /dev/%s", nic->name);
 811         driver_pkt_dequeue(&nic->tx_head, &nic->tx_tail);
 812 }
 813
 814 struct packet_q * driver_tx_head(struct nic * nic)
 815 {
 816         debug_print("device /dev/%s", nic->name);
 817
 818         if (!nic->tx_head)
 819                 return NULL;
 820         return nic->tx_head;
 821 }