Arduino-Library-SoftDhcp/SoftDhcp.cpp

392 lines
11 KiB
C++

#include "SoftDhcp.h"
#define PORT_BOOTPS 67
#define PORT_BOOTPC 68
const uint32_t DHCP_MAGIC = htonl(0x63825363);
const IPAddress BROADCAST(0xffffffffUL);
SoftDhcp::SoftDhcp() {
this->leaseHandler = nullptr;
this->offerHandler = nullptr;
this->errorHandler = nullptr;
this->reqoptptr = this->reqopt;
this->reqopt[0] = DhcpOption::END;
}
void SoftDhcp::initializePacket() {
memset((uint8_t *) &this->packet, 0, sizeof(DhcpPacket));
WiFi.macAddress((uint8_t *) this->packet.chaddr);
this->packet.magic = DHCP_MAGIC;
this->clearOptions();
}
void SoftDhcp::clearOptions() {
this->packet.options[0] = DhcpOption::END;
this->optptr = this->packet.options;
}
bool SoftDhcp::addOption(uint8_t type, uint8_t len, uint8_t *value) {
if (this->optptr + len + 3 >= this->packet.options + 308) { // 3 = type+len+end
return false;
}
*(this->optptr++) = type;
*(this->optptr++) = len;
memcpy(this->optptr, value, len);
this->optptr += len;
*this->optptr = DhcpOption::END;
return true;
}
bool SoftDhcp::addOption(uint8_t type, uint8_t value) {
return this->addOption(type, 1, &value);
}
bool SoftDhcp::appendRequestOptions() {
uint8_t *ro = this->reqopt;
while (ro < this->reqoptptr) {
uint8_t rotype = *(ro++);
uint8_t rolen = *(ro++);
if (!this->addOption(rotype, rolen, ro)) {
return false;
}
ro += rolen;
}
return true;
}
bool SoftDhcp::addRequestOption(uint8_t type, uint8_t len, uint8_t *value) {
if (this->reqoptptr + len + 3 >= this->reqopt + 308) { // 3 = type+len+end
return false;
}
*(this->reqoptptr++) = type;
*(this->reqoptptr++) = len;
memcpy(this->reqoptptr, value, len);
this->reqoptptr += len;
*this->reqoptptr = DhcpOption::END;
return true;
}
uint8_t *SoftDhcp::getOption(uint8_t type) {
uint8_t *optptr = this->packet.options;
while (optptr < this->packet.options + 305 && *optptr != DhcpOption::END && *optptr != DhcpOption::PAD) {
if (*optptr == type) {
return optptr;
} else {
uint8_t optlen = *(optptr+1);
optptr += optlen + 2;
}
}
return nullptr;
}
void SoftDhcp::hostname(const char *name) {
this->addRequestOption(DhcpOption::HOSTNAME, strlen(name), (uint8_t *) name);
}
void SoftDhcp::hostname(String& name) {
this->addRequestOption(DhcpOption::HOSTNAME, name.length(), (uint8_t *) name.c_str());
}
ssize_t SoftDhcp::getOption(uint8_t type, void *buf, size_t len, ssize_t item) {
uint8_t *optptr = this->packet.options;
while (optptr < this->packet.options + 305 && *optptr != DhcpOption::END && *optptr != DhcpOption::PAD) {
uint8_t opt = *(optptr++);
uint8_t optlen = *(optptr++);
if (opt != type) {
optptr += optlen;
continue;
}
if (item < 0) {
if (optlen > len) {
return -2;
} else {
memcpy(buf, optptr, optlen);
return optlen;
}
} else {
if (optlen < len * (item + 1) || optlen % len != 0) {
return -2;
} else {
memcpy(buf, optptr + len * item, len);
return len;
}
}
}
return -1;
}
ssize_t SoftDhcp::getOption(uint8_t type, void *buf, size_t len) {
return this->getOption(type, buf, len, -1);
}
void SoftDhcp::request() {
uint8_t macbuf[7];
if (this->last_xid == 0) {
// Send DHCP DISCOVER
this->initializePacket();
this->request_sent = true;
while ((this->last_xid = random(0xffffffffUL)) == 0);
this->packet.op = 1;
this->packet.htype = 1;
this->packet.hlen = 6;
this->packet.xid = this->last_xid;
this->addOption(DhcpOption::MSG_TYPE, DhcpMessageType::DHCPDISCOVER);
macbuf[0] = 1; // 1 = ethernet
WiFi.macAddress(macbuf+1);
this->addOption(DhcpOption::CLIENT_ID, 7, macbuf);
this->addOption(DhcpOption::PARAMETER_LIST, this->prl_count, this->prl);
this->appendRequestOptions();
this->udp.beginPacket(this->serverIP, PORT_BOOTPS);
this->udp.write((uint8_t *) &this->packet, sizeof(DhcpPacket));
this->udp.endPacket();
} else {
this->getOption(DhcpOption::SERVER_ID, &this->sid.addr, 4);
if (this->packet.yiaddr.addr) {
this->yiaddr.addr = this->packet.yiaddr.addr;
}
// Send DHCP REQUEST
this->initializePacket();
this->request_sent = true;
this->packet.op = 1;
this->packet.htype = 1;
this->packet.hlen = 6;
this->packet.xid = this->last_xid;
this->addOption(DhcpOption::MSG_TYPE, DhcpMessageType::DHCPREQUEST);
macbuf[0] = 1; // 1 = ethernet
WiFi.macAddress(macbuf+1);
this->addOption(DhcpOption::CLIENT_ID, 7, macbuf);
this->addOption(DhcpOption::PARAMETER_LIST, this->prl_count, this->prl);
this->addOption(DhcpOption::SERVER_ID, 4, this->sid.octets);
this->addOption(DhcpOption::ADDRESS_REQUEST, 4, this->yiaddr.octets);
this->appendRequestOptions();
this->udp.beginPacket(this->serverIP, PORT_BOOTPS);
this->udp.write((uint8_t *) &this->packet, sizeof(DhcpPacket));
this->udp.endPacket();
}
}
int32_t SoftDhcp::getTimeoutOption(uint8_t option, int32_t def) {
uint32_t time;
if (this->getOption(option, (uint8_t *) &time, 4) < 0) {
time = def;
} else {
time = ntohl(time);
}
if (time > INT32_MAX) {
time = INT32_MAX;
}
return (int32_t) time;
}
void SoftDhcp::respond() {
if (this->packet.magic != DHCP_MAGIC) {
// not a DHCP packet
return;
}
if (this->packet.xid != this->last_xid) {
// not my DHCP packet
return;
}
uint8_t type;
if (this->getOption(DhcpOption::MSG_TYPE, &type, 1) != 1) {
// Malformed DHCP packet
return;
}
switch (type) {
case DhcpMessageType::DHCPOFFER:
if (this->offerHandler) {
this->offerHandler();
}
this->request();
return;
case DhcpMessageType::DHCPACK:
this->request_sent = false;
this->last_update = millis();
this->clientIP = this->packet.yiaddr.addr;
if (packet.giaddr.addr) {
this->serverIP = this->packet.giaddr.addr;
} else {
this->serverIP = this->packet.siaddr.addr;
}
uint32_t addr;
this->getOption(DhcpOption::SUBNET_MASK, &addr, 4);
this->netmask = addr;
this->getOption(DhcpOption::ROUTER, &addr, 4, 0);
this->routerIP = addr;
this->getOption(DhcpOption::DOMAIN_SERVER, &addr, 4, 0);
this->dnsIP1 = addr;
this->getOption(DhcpOption::DOMAIN_SERVER, &addr, 4, 1);
this->dnsIP2 = addr;
WiFi.config(this->clientIP, this->routerIP, this->netmask, this->dnsIP1, this->dnsIP2);
// Set countdowns
this->renew_countdown = this->getTimeoutOption(DhcpOption::RENEWAL_TIME, 1800);
this->rebind_countdown = this->getTimeoutOption(DhcpOption::REBINDING_TIME, 3600);
this->expire_countdown = this->getTimeoutOption(DhcpOption::ADDRESS_TIME, INT32_MAX);
if (this->leaseHandler) {
this->leaseHandler();
}
return;
default:
this->begin();
if (this->errorHandler) {
this->errorHandler();
}
return;
}
}
void SoftDhcp::requestOption(uint8_t option) {
if (!memchr(this->prl, option, this->prl_count)) {
this->prl[this->prl_count++] = option;
}
}
void SoftDhcp::begin() {
this->request_sent = false;
this->last_update = millis();
this->last_xid = 0;
this->prl_count = 0;
this->requestOption(DhcpOption::SUBNET_MASK);
this->requestOption(DhcpOption::ROUTER);
this->requestOption(DhcpOption::DOMAIN_SERVER);
this->serverIP = BROADCAST;
this->clientIP = BROADCAST;
this->udp.begin(PORT_BOOTPC);
// Disable builtin dhcpc
WiFi.config(BROADCAST, BROADCAST, BROADCAST, 0, 0);
}
void SoftDhcp::update() {
if (this->request_sent) {
size_t len = this->udp.parsePacket();
if (len < 240) {
// not a DHCP packet
return;
}
// TODO check length
this->udp.read((uint8_t *) &this->packet, sizeof(DhcpPacket));
this->respond();
} else {
// Count down renew countdown if more than 1s has passed
uint32_t now = millis();
uint32_t td = (now - this->last_update) / 1000UL;
if (td) {
this->last_update = now;
this->renew_countdown -= td;
this->rebind_countdown -= td;
this->expire_countdown -= td;
}
if (this->rebind_countdown <= 0) {
this->serverIP = BROADCAST;
this->rebind_countdown = INT_MAX;
}
if (this->expire_countdown <= 0) {
this->begin();
if (this->expireHandler) {
this->expireHandler();
}
}
if (this->renew_countdown <= 0) {
this->request();
this->renew_countdown = 60;
}
}
}
IPAddress SoftDhcp::localIP() {
return this->clientIP;
}
IPAddress SoftDhcp::subnetMask() {
return this->netmask;
}
IPAddress SoftDhcp::gatewayIP() {
return this->routerIP;
}
IPAddress SoftDhcp::dnsIP(uint8_t dns_no) {
if (dns_no == 0) {
return this->dnsIP1;
} else if (dns_no == 1) {
return this->dnsIP2;
} else {
return IPAddress(0);
}
}
void SoftDhcp::onLease(void (*handler)()) {
this->leaseHandler = handler;
}
void SoftDhcp::onOffer(void (*handler)()) {
this->offerHandler = handler;
}
void SoftDhcp::onError(void (*handler)()) {
this->errorHandler = handler;
}
void SoftDhcp::onExpire(void (*handler)()) {
this->expireHandler = handler;
}
void SoftDhcp::dump() {
uint8_t type;
this->getOption(DhcpOption::MSG_TYPE, &type, 1);
switch (type) {
case DhcpMessageType::DHCPDISCOVER:
Serial.println("DHCPDISCOVER:");
break;
case DhcpMessageType::DHCPOFFER:
Serial.println("DHCPOFFER:");
break;
case DhcpMessageType::DHCPREQUEST:
Serial.println("DHCPREQUEST:");
break;
case DhcpMessageType::DHCPACK:
Serial.println("DHCPACK:");
break;
case DhcpMessageType::DHCPNAK:
Serial.println("DHCPNAK:");
break;
default:
Serial.println("OTHER:");
break;
}
Serial.printf(" op: %d\r\n", this->packet.op);
Serial.printf(" htype: %d\r\n", this->packet.htype);
Serial.printf(" hlen: %d\r\n", this->packet.hlen);
Serial.printf(" hops: %d\r\n", this->packet.hops);
Serial.printf(" xid: %08x\r\n", this->packet.xid);
Serial.printf(" secs: %d\r\n", this->packet.secs);
Serial.printf(" flags: %d\r\n", this->packet.flags);
Serial.printf(" ciaddr: %d.%d.%d.%d\r\n", this->packet.ciaddr.octets[0], this->packet.ciaddr.octets[1], this->packet.ciaddr.octets[2], this->packet.ciaddr.octets[3]);
Serial.printf(" yiaddr: %d.%d.%d.%d\r\n", this->packet.yiaddr.octets[0], this->packet.yiaddr.octets[1], this->packet.yiaddr.octets[2], this->packet.yiaddr.octets[3]);
Serial.printf(" siaddr: %d.%d.%d.%d\r\n", this->packet.siaddr.octets[0], this->packet.siaddr.octets[1], this->packet.siaddr.octets[2], this->packet.siaddr.octets[3]);
Serial.printf(" giaddr: %d.%d.%d.%d\r\n", this->packet.giaddr.octets[0], this->packet.giaddr.octets[1], this->packet.giaddr.octets[2], this->packet.giaddr.octets[3]);
Serial.printf(" chaddr: %s\r\n", this->packet.chaddr);
Serial.printf(" sname: %s\r\n", this->packet.sname);
Serial.printf(" file: %s\r\n", this->packet.file);
Serial.printf(" magic: %08x\r\n", this->packet.magic);
Serial.println(" options:");
uint8_t *optptr = this->packet.options;
while (optptr < this->packet.options + 305 && *optptr != DhcpOption::END && *optptr != DhcpOption::PAD) {
uint8_t opttype = *(optptr++);
uint8_t optlen = *(optptr++);
Serial.printf(" option %02x length %02x:\r\n ", opttype, optlen);
for (uint8_t i = 0; i < optlen; ++i) {
Serial.printf(" %02x", optptr[i]);
}
Serial.println();
optptr += optlen;
}
}