static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type)
{
    int r = 0, optlen = 0, offset = 0;
    struct pico_ip4 broadcast = {
        0
    }, dns = {
        0
    }, destination = {
        .addr = 0xFFFFFFFF
    };
    struct pico_dhcp_hdr *hdr = NULL;

    dns.addr = DHCP_SERVER_OPENDNS;
    broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr);

    optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER
             + PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END;
    hdr = PICO_ZALLOC(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen);
    if (!hdr) {
        return;
    }

    hdr->op = PICO_DHCP_OP_REPLY;
    hdr->htype = PICO_DHCP_HTYPE_ETH;
    hdr->hlen = PICO_SIZE_ETH;
    hdr->xid = dhcpn->xid;
    hdr->yiaddr = dhcpn->ciaddr.addr;
    hdr->siaddr = dhcpn->dhcps->server_ip.addr;
    hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
    memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH);

    /* options */
    offset += pico_dhcp_opt_msgtype(DHCP_OPT(hdr,offset), msg_type);
    offset += pico_dhcp_opt_serverid(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip);
    offset += pico_dhcp_opt_leasetime(DHCP_OPT(hdr,offset), dhcpn->dhcps->lease_time);
    offset += pico_dhcp_opt_netmask(DHCP_OPT(hdr,offset), &dhcpn->dhcps->netmask);
    offset += pico_dhcp_opt_router(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip);
    offset += pico_dhcp_opt_broadcast(DHCP_OPT(hdr,offset), &broadcast);
    offset += pico_dhcp_opt_dns(DHCP_OPT(hdr,offset), &dns);
    offset += pico_dhcp_opt_end(DHCP_OPT(hdr,offset));

    if (dhcpn->bcast == 0)
        destination.addr = hdr->yiaddr;
    else {
        hdr->flags |= short_be(PICO_DHCP_FLAG_BROADCAST);
        destination.addr = broadcast.addr;
    }

    r = pico_socket_sendto(dhcpn->dhcps->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen), &destination, PICO_DHCP_CLIENT_PORT);
    if (r < 0)
        dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err));

    PICO_FREE(hdr);

    return;
}

static inline void parse_opt_msgtype(struct pico_dhcp_opt *opt, uint8_t *msgtype)
{
    if (opt->code == PICO_DHCP_OPT_MSGTYPE) {
        *msgtype = opt->ext.msg_type.type;
        dhcps_dbg("DHCP server: message type %u\n", msgtype);
    }
}

static inline void parse_opt_reqip(struct pico_dhcp_opt *opt, struct pico_ip4 *reqip)
{
    if (opt->code == PICO_DHCP_OPT_REQIP)
        reqip->addr = opt->ext.req_ip.ip.addr;
}

static inline void parse_opt_serverid(struct pico_dhcp_opt *opt,  struct pico_ip4 *serverid)
{
    if (opt->code == PICO_DHCP_OPT_SERVERID)
        *serverid = opt->ext.server_id.ip;
}

static inline void dhcps_make_reply_to_request_msg(struct pico_dhcp_server_negotiation *dhcpn, int bound_valid_flag)
{
    if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && bound_valid_flag)
        dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);

    if (dhcpn->state == PICO_DHCP_STATE_OFFER) {
        dhcpn->state = PICO_DHCP_STATE_BOUND;
        dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
    }
}

static inline void dhcps_make_reply_to_discover_or_request(struct pico_dhcp_server_negotiation *dhcpn,  uint8_t msgtype, int bound_valid_flag)
{
    if (PICO_DHCP_MSG_DISCOVER == msgtype) {
        dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER);
        dhcpn->state = PICO_DHCP_STATE_OFFER;
    } else if (PICO_DHCP_MSG_REQUEST == msgtype) {
        dhcps_make_reply_to_request_msg(dhcpn, bound_valid_flag);
    }
}

static inline void dhcps_parse_options_loop(struct pico_dhcp_server_negotiation *dhcpn, struct pico_dhcp_hdr *hdr)
{
    struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0);
    uint8_t msgtype = 0;
    struct pico_ip4 reqip = {
        0
    }, server_id = {
        0
    };

    do {
        parse_opt_msgtype(opt, &msgtype);
        parse_opt_reqip(opt, &reqip);
        parse_opt_serverid(opt, &server_id);
    } while (pico_dhcp_next_option(&opt));
    dhcps_make_reply_to_discover_or_request(dhcpn, msgtype, (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr));
}

static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, uint32_t len)
{
    int32_t optlen = (int32_t)(len - sizeof(struct pico_dhcp_hdr));
    struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
    struct pico_dhcp_server_negotiation *dhcpn = NULL;
    struct pico_device *dev = NULL;

    if (!pico_dhcp_are_options_valid(DHCP_OPT(hdr,0), optlen))
        return;

    dev = pico_ipv4_link_find(&s->local_addr.ip4);
    dhcpn = pico_dhcp_server_find_negotiation(hdr->xid);
    if (!dhcpn)
        dhcpn = pico_dhcp_server_add_negotiation(dev, hdr);

    if (!ip_address_is_in_dhcp_range(dhcpn, dhcpn->ciaddr.addr))
        return;

    dhcps_parse_options_loop(dhcpn, hdr);

}
Exemple #2
0
static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type)
{
  int r = 0, optlen = 0, offset = 0;
  struct pico_ip4 broadcast = {0}, dns = {0}, destination = { .addr = 0xFFFFFFFF};
  struct pico_dhcp_hdr *hdr = NULL;

  dns.addr = DHCP_SERVER_OPENDNS;
  broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr);

  optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER 
          + PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END;
  hdr = pico_zalloc(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen);

  hdr->op = PICO_DHCP_OP_REPLY;
  hdr->htype = PICO_DHCP_HTYPE_ETH;
  hdr->hlen = PICO_SIZE_ETH;
  hdr->xid = dhcpn->xid;
  hdr->yiaddr = dhcpn->ciaddr.addr;
  hdr->siaddr = dhcpn->dhcps->server_ip.addr;
  hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
  memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH);

  /* options */
  offset += pico_dhcp_opt_msgtype(&hdr->options[offset], msg_type);
  offset += pico_dhcp_opt_serverid(&hdr->options[offset], &dhcpn->dhcps->server_ip);
  offset += pico_dhcp_opt_leasetime(&hdr->options[offset], dhcpn->dhcps->lease_time);
  offset += pico_dhcp_opt_netmask(&hdr->options[offset], &dhcpn->dhcps->netmask);
  offset += pico_dhcp_opt_router(&hdr->options[offset], &dhcpn->dhcps->server_ip);
  offset += pico_dhcp_opt_broadcast(&hdr->options[offset], &broadcast);
  offset += pico_dhcp_opt_dns(&hdr->options[offset], &dns);
  offset += pico_dhcp_opt_end(&hdr->options[offset]);

  destination.addr = hdr->yiaddr;
  r = pico_socket_sendto(dhcpn->dhcps->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen), &destination, PICO_DHCP_CLIENT_PORT);
  if (r < 0)
    dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err));

  return;
}

static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, uint32_t len)
{
  uint8_t msgtype = 0;
  int32_t optlen = (int32_t)(len - sizeof(struct pico_dhcp_hdr));
  struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
  struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)hdr->options;
  struct pico_dhcp_server_negotiation *dhcpn = NULL;
  struct pico_ip4 reqip = {0}, server_id = {0};
  struct pico_device *dev = NULL;

  if (!pico_dhcp_are_options_valid(hdr->options, optlen))
    return;

  dev = pico_ipv4_link_find(&s->local_addr.ip4);
  dhcpn = pico_dhcp_server_find_negotiation(hdr->xid);
  if (!dhcpn)
    dhcpn = pico_dhcp_server_add_negotiation(dev, hdr);
 
  if (!ip_inrange(dhcpn->ciaddr.addr))
    return;

  do {
    switch (opt->code)
    {
      case PICO_DHCP_OPT_PAD:
        break;

      case PICO_DHCP_OPT_END:
        break;

      case PICO_DHCP_OPT_MSGTYPE:
        msgtype = opt->ext.msg_type.type;
        dhcps_dbg("DHCP server: message type %u\n", msgtype);
        break;

      case PICO_DHCP_OPT_REQIP:
        reqip = opt->ext.req_ip.ip;
        dhcps_dbg("DHCP server: requested IP %08X\n", reqip.addr);
        break;

      case PICO_DHCP_OPT_SERVERID:
        server_id = opt->ext.server_id.ip;
        dhcps_dbg("DHCP server: server ID %08X\n", server_id.addr);
        break;        

      default:
        dhcps_dbg("DHCP server WARNING: unsupported option %u\n", opt->code);
        break;
    }
  } while (pico_dhcp_next_option(&opt));

  switch (msgtype)
  {
    case PICO_DHCP_MSG_DISCOVER:
      dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER);
      dhcpn->state = PICO_DHCP_STATE_OFFER;
      break;

    case PICO_DHCP_MSG_REQUEST:
      if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr))
        dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
      if (dhcpn->state == PICO_DHCP_STATE_OFFER) {
        dhcpn->state = PICO_DHCP_STATE_BOUND;
        dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
      }
      break;

    default:
      dhcps_dbg("DHCP server WARNING: unsupported message type %u\n", msgtype);
      break;
  }
  return;
}