--- /dev/null
+#include "uldy_server.h"
+
+using namespace std;
+
+extern CidPool cidPool;
+
+bool AddressPool::blockAddress(long sn, int addr) {
+ if (!assignedAddrs.contains(sn) && !assignedAddrs.values().contains(addr)) {
+ assignedAddrs.insert(sn, addr);
+ holes.remove(addr);
+ }
+ return false;
+}
+
+int AddressPool::getFreeAddress(long sn) {
+// return 60; // DOCASNE
+ if (assignedAddrs.size() == MAX_ADDR) return 0;
+ int retval;
+ if (holes.size() == 0) {
+ retval = assignedAddrs.size() + 1;
+ } else {
+ QMutableSetIterator<int> it(holes);
+ it.next();
+ retval = it.value();
+ it.remove();
+ }
+ assignedAddrs.insert(sn, retval);
+ return retval;
+}
+
+void AddressPool::freeAddress(long sn) {
+// return; // DOCASNE
+ if (assignedAddrs.contains(sn)) {
+ int addr = assignedAddrs.value(sn);
+ if (addr != assignedAddrs.size())
+ holes.insert(addr);
+ assignedAddrs.remove(sn);
+ }
+}
+AddressPool addressPool;
+
+
+void ULDYServer::sendNewAddress(int addr, long sn) const {
+ int ret;
+ QByteArray buf_out;
+ ul_fd_t ul_fd = ul_open(UL_DEV_NAME, NULL);
+ if (ul_fd == UL_FD_INVALID) {
+ logToFile("[EE] ULDYSRV - chyba pri zasilani adresy: neplatny deskriptor...");
+ return;
+ }
+ buf_out.append(ULNCS_SET_ADDR); /* SN0 SN1 SN2 SN3 NEW_ADR */
+ buf_out += sn2buf(sn);
+ buf_out.append(addr);
+ ret = ul_send_command(ul_fd, 0, UL_CMD_NCS, UL_BFL_NORE, (uchar*)buf_out.constData(), buf_out.size()); //UL_BFL_ARQ
+// logToFile(QString("[II] odeslana zprava, status: %1, len: %2 data %3").arg(ret).arg(buf_out.size()).arg(buf_out.toHex().constData()));
+ ul_close(ul_fd);
+}
+
+void ULDYServer::requestIdentification(int addr) const {
+ int ret;
+ QByteArray buf_out;
+ ul_fd_t ul_fd = ul_open(UL_DEV_NAME, NULL);
+ if (ul_fd == UL_FD_INVALID) {
+ logToFile("[EE] ULDYSRV - chyba pri dotazu na identifikaci: neplatny deskriptor...");
+ return;
+ }
+ buf_out.append(ULNCS_SID_RQ);
+ ret = ul_send_command(ul_fd, addr, UL_CMD_NCS, UL_BFL_NORE, (uchar*)buf_out.constData(), buf_out.size()); //UL_BFL_ARQ
+ logToFile(QString("[II] odeslana zadost o identifikaci na adresu %1").arg(addr));
+ ul_close(ul_fd);
+}
+
+void ULDYServer::processPDOMessage(int saddr, int cid, int data) {
+// logToFile(QString("PDO msg: CID = %1, DATA = %2").arg(cid).arg(data));
+ if (nodes.contains(saddr)) {
+ Node &node = nodes.getNodeRef(saddr);
+ if (cid == HEARTBEAT_CID) {
+
+ logToFile(QString("[II] - HEARTBEAT from address %1").arg(saddr));
+ node.refreshLastUpdate();
+
+ } else {
+
+ try {
+ NodeVar tmpVar = node.hasCid(cid);
+ float fdata = ((float)data) / pow(10.0, (double)tmpVar.getDecimalPlaces());
+ valueCache.insertValue(node.getSN(), tmpVar.getId(), fdata);
+ } catch (NodeVarError) { }
+ node.refreshLastUpdate();
+
+ }
+ } else if (saddr != NOT_ASSIGNED_ADDR) {
+
+ requestIdentification(saddr);
+
+ }
+// logToFile(QString("PDO zpracovana %1").arg((unsigned long)QThread::currentThreadId()));
+}
+
+void ULDYServer::stopServer(void) {
+ logToFile("[II] ULDYSRV - stopping uldy server...");
+ this->running = false;
+}
+
+void ULDYServer::run() {
+ int ret;
+ int addr, saddr, msg_len;
+ long sn;
+ QByteArray buf_in;
+ ul_msginfo msginfo;
+
+ QTime timer;
+ timer.start();
+#ifdef TEST_RUN
+ Node *newNode = new Node(30, 102);
+ this->nodes.insert(30, newNode);
+
+ while (this->running) {
+ sleep(5);
+ int data = getRandom();
+ int cid = 16;
+ processPDOMessage(30, cid, data);
+ }
+#else
+ ul_fd_t ul_fd = ul_open(UL_DEV_NAME, NULL);
+ if (ul_fd == UL_FD_INVALID) {
+ logToFile("[EE] ULDYSRV - neplatny deskriptor");
+ return;
+ }
+
+ memset(&msginfo, 0, sizeof(ul_msginfo));
+ ul_addfilt(ul_fd, &msginfo);
+
+ logToFile("[II] ULDYSRV - cekam na zpravu...");
+
+ while (this->running) { // main loop
+
+ while ((ul_fd_wait(ul_fd, 1) <= 0) && this->running) ;
+
+ if ((ul_inepoll(ul_fd) > 0) && (ul_acceptmsg(ul_fd, &msginfo)) >= 0) {
+ if (msginfo.cmd == UL_CMD_NCS) {
+ msg_len = msginfo.len;
+ saddr = msginfo.sadr;
+ buf_in.resize(msg_len);
+ ret = ul_read(ul_fd, buf_in.data(), msg_len); // nacteni SN do bufferu
+// logToFile(QString("[II] prijata zprava, sadr: %1, dadr: %2, len: %3 data %4").arg(msginfo.sadr).arg(msginfo.dadr).arg(msginfo.len).arg(buf_in.toHex().constData()));
+ ul_freemsg(ul_fd);
+ switch ((uchar)buf_in[0]) {
+ case ULNCS_ADR_RQ:
+ sn = buf2sn(buf_in);
+ if (sn == -1) {
+ logToFile(QString("[WW] ULDYSRV - neplatna zadost o adresu, ocekavam delku minimalne 5").arg(msg_len));
+ break;
+ }
+// logToFile(QString("[II] prijmam zadost o adresu, SN %1").arg(sn));
+ if ((addr = addressPool.getFreeAddress(sn)) != 0) {
+ logToFile(QString("[II] ULDYSRV - sending new address \"%1\" to SN \"%2\"...").arg(addr).arg(sn));
+ if (!(this->nodes.contains(addr))) {
+ sendNewAddress(addr, sn);
+ Node *newNode = new Node(addr, sn);
+ this->nodes.insert(addr, newNode);
+ logToFile(QString("[II] mapa nodu nyni obsahuje %1 polozek...").arg(this->nodes.size()));
+ } else { logToFile("nod uz tam je"); }
+ } else {
+ logToFile("[WW] ULDYSRV - address pool is full, try again later, please...");
+ }
+ break;
+ case ULNCS_SID_RPLY:
+ sn = buf2sn(buf_in);
+ logToFile(QString("[II] - received identification from unknown node, addr %1").arg(saddr));
+ if (!(this->nodes.contains(saddr))) {
+ Node *newNode = new Node(saddr, sn);
+ addressPool.blockAddress(sn, saddr);
+ this->nodes.insert(saddr, newNode);
+ logToFile(QString("[II] mapa nodu nyni obsahuje %1 polozek...").arg(this->nodes.size()));
+ } else { logToFile("nod uz tam je"); }
+ break;
+
+ default:
+ break;
+ } /* switch */
+
+ } else if (msginfo.cmd == UL_CMD_PDO) {
+ msg_len = msginfo.len;
+ saddr = msginfo.sadr;
+ buf_in.resize(msg_len);
+ ret = ul_read(ul_fd, buf_in.data(), msg_len);
+ ul_freemsg(ul_fd);
+
+ buf_in = buf_in.mid(PDO_SKIP_BYTES);
+
+ while (1) {
+ bool success = true;
+
+ int cid = takeCid(buf_in, &success);
+ if (!success) break;
+
+ int len = takeLen(buf_in, &success);
+ if (!success) break;
+
+ int data = takeData(buf_in, len, &success);
+ if (!success) break;
+
+ processPDOMessage(saddr, cid, data);
+ }
+ } else
+ ul_freemsg(ul_fd);
+
+ } /* ul_inepool && ul_acceptmsg */
+
+ } /* while(running) */
+#endif
+}
+
+int ULDYServer::takeCid(QByteArray &buf, bool *success) const {
+ if (buf.size() < 2) { *success = false; /*logToFile("nedostatecna delka CID");*/ return 0; }
+ int ret = buf2uint16(buf);
+ buf = buf.mid(2);
+ *success = true;
+ return ret;
+}
+
+int ULDYServer::takeLen(QByteArray &buf, bool *success) const {
+// return takeCid(buf, success);
+ if (buf.size() < 2) { *success = false; /*logToFile("nedostatecna delka LEN");*/ return 0; }
+// int ret = buf[0];
+ int ret = buf2uint16(buf);
+ buf = buf.mid(2);
+ *success = true;
+ return ret;
+}
+
+int ULDYServer::takeData(QByteArray &buf, int len, bool *success) const {
+ if (buf.size() < len) { *success = false; return 0; }
+ int ret = 0;
+ for (int i = 0; i < len; i++) {
+ ret += (uchar)buf[i]<<(i * 8);
+ }
+ buf = buf.mid(len);
+ *success = true;
+ return ret;
+}
+
+const NodeMap &ULDYServer::getNodes() const {
+ return this->nodes;
+}
+
+NodeMap &ULDYServer::getNodesRef() {
+ return this->nodes;
+}
+
+ValueCache &ULDYServer::getValueCacheRef() {
+ return this->valueCache;
+}
+
+void ULDYServer::tryRemoveNodes() {
+ NodeMapMutableIterator it(this->nodes);
+ QDateTime curr, last;
+ while (it.hasNext()) {
+ it.next();
+ int addr = it.key();
+ curr = currentDateTime();
+ last = this->nodes.getNodeRef(addr).getLastUpdate();
+ last.setTimeSpec(Qt::UTC);
+ if ((curr.toTime_t() - last.toTime_t()) > NODE_CLEANER_PERIOD_SECS) {
+ logToFile(QString("[II] - Removing node SN %1...").arg(this->nodes.getNodeRef(addr).getSN()));
+ addressPool.freeAddress(this->nodes.getNodeRef(addr).getSN());
+ cidPool.freeCids(this->nodes.getNodeRef(addr).getUsedCidsRef());
+ Node *tmpNode = this->nodes.value(addr);
+ this->nodes.remove(addr);
+ delete tmpNode;
+ }
+ }
+}
+
projects / hydro.git / blobdiff