From c132fadb4461d9d0cdb8ac7c5cac223f8bc681c9 Mon Sep 17 00:00:00 2001 From: Jessy Diamond Exum Date: Tue, 10 Oct 2017 01:16:03 -0700 Subject: [PATCH] J2534_WIN: Added ISO15765 multiframe messages compliant with new docs. --- .../windows/pandaJ2534DLL/J2534Connection.cpp | 8 +++ .../windows/pandaJ2534DLL/J2534Connection.h | 2 + .../J2534Connection_ISO15765.cpp | 16 +++--- .../pandaJ2534DLL/J2534Connection_ISO15765.h | 3 +- drivers/windows/pandaJ2534DLL/MessageTx.h | 6 ++- .../pandaJ2534DLL/MessageTx_ISO15765.cpp | 52 ++++++++++++------- .../pandaJ2534DLL/MessageTx_ISO15765.h | 8 ++- .../pandaJ2534DLL/PandaJ2534Device.cpp | 51 ++++++++++++++---- 8 files changed, 102 insertions(+), 44 deletions(-) diff --git a/drivers/windows/pandaJ2534DLL/J2534Connection.cpp b/drivers/windows/pandaJ2534DLL/J2534Connection.cpp index 094d713af..ae7fb0c46 100644 --- a/drivers/windows/pandaJ2534DLL/J2534Connection.cpp +++ b/drivers/windows/pandaJ2534DLL/J2534Connection.cpp @@ -116,6 +116,14 @@ void J2534Connection::schedultMsgTx(std::shared_ptr msgout) { } } +void J2534Connection::rescheduleExistingTxMsgs() { + if (auto panda_ps = this->panda_dev.lock()) { + synchronized(staged_writes_lock) { + panda_ps->registerConnectionTx(shared_from_this()); + } + } +} + void J2534Connection::processMessageReceipt(const J2534Frame& msg) { //TX_MSG_TYPE should be set in RxStatus if (!check_bmask(msg.RxStatus, TX_MSG_TYPE)) return; diff --git a/drivers/windows/pandaJ2534DLL/J2534Connection.h b/drivers/windows/pandaJ2534DLL/J2534Connection.h index 763dd7178..134faaadf 100644 --- a/drivers/windows/pandaJ2534DLL/J2534Connection.h +++ b/drivers/windows/pandaJ2534DLL/J2534Connection.h @@ -72,6 +72,8 @@ public: void schedultMsgTx(std::shared_ptr msgout); + void rescheduleExistingTxMsgs(); + std::shared_ptr getPandaDev() { if (auto panda_dev_sp = this->panda_dev.lock()) return panda_dev_sp; diff --git a/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.cpp b/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.cpp index d36e7e032..9382380e9 100644 --- a/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.cpp +++ b/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.cpp @@ -63,30 +63,30 @@ void J2534Connection_ISO15765::processMessage(const J2534Frame& msg) { switch (msg_get_type(msg, addrlen)) { case FRAME_FLOWCTRL: { - /*if (msg.Data.size() < addrlen + 3) return; + if (msg.Data.size() < addrlen + 3) return; uint8_t flow_status = msg.Data[addrlen] & 0x0F; uint8_t block_size = msg.Data[addrlen + 1]; uint8_t st_min = msg.Data[addrlen + 2]; + + auto txConvo = std::static_pointer_cast(this->txbuff.back()); switch (flow_status) { case FLOWCTRL_CONTINUE: { if (st_min > 0xF9) break; if (st_min >= 0xf1 && st_min <= 0xf9) { - this->rxConversations[fid]->tx_flowcontrol(block_size, std::chrono::microseconds((st_min & 0x0F) * 100)); + txConvo->tx_flowcontrol(block_size, std::chrono::microseconds((st_min & 0x0F) * 100), block_size==0); } else { - this->rxConversations[fid]->tx_flowcontrol(block_size, std::chrono::microseconds(st_min * 1000)); - } - if (auto panda_dev_sp = this->panda_dev.lock()) { - panda_dev_sp->registerMultiPartTx(this->conversations[fid]); + txConvo->tx_flowcontrol(block_size, std::chrono::microseconds(st_min * 1000), block_size==0); } + this->rescheduleExistingTxMsgs(); break; } case FLOWCTRL_WAIT: - this->rxConversations[fid]->tx_flowcontrol(0, std::chrono::microseconds(0)); + txConvo->tx_flowcontrol(0, std::chrono::microseconds(0)); break; case FLOWCTRL_ABORT: this->rxConversations[fid] = nullptr; break; - }*/ + } break; } case FRAME_SINGLE: diff --git a/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.h b/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.h index d290c8f3b..accad9c28 100644 --- a/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.h +++ b/drivers/windows/pandaJ2534DLL/J2534Connection_ISO15765.h @@ -12,7 +12,7 @@ typedef struct { std::string remaining_payload; } PRESTAGED_WRITE; -class J2534Connection_ISO15765 : public J2534Connection { //J2534Connection_CAN { +class J2534Connection_ISO15765 : public J2534Connection { public: J2534Connection_ISO15765( std::shared_ptr panda_dev, @@ -49,6 +49,5 @@ public: } private: - std::array staged_writes; std::array, 10> rxConversations; }; diff --git a/drivers/windows/pandaJ2534DLL/MessageTx.h b/drivers/windows/pandaJ2534DLL/MessageTx.h index 6a9ff4801..e27053672 100644 --- a/drivers/windows/pandaJ2534DLL/MessageTx.h +++ b/drivers/windows/pandaJ2534DLL/MessageTx.h @@ -11,7 +11,7 @@ public: MessageTx( std::shared_ptr connection, PASSTHRU_MSG& to_send - ) : connection(connection), fullmsg(to_send) { };//next(nullptr), + ) : connection(connection), fullmsg(to_send) { }; virtual BOOL sendNextFrame() = 0; @@ -19,8 +19,10 @@ public: virtual BOOL isFinished() = 0; - //std::shared_ptr next; + virtual BOOL txReady() = 0; + std::weak_ptr connection; J2534Frame fullmsg; + std::chrono::microseconds separation_time; }; diff --git a/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.cpp b/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.cpp index eeb4a25ba..e788c65a0 100644 --- a/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.cpp +++ b/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.cpp @@ -6,7 +6,7 @@ MessageTx_ISO15765::MessageTx_ISO15765( std::shared_ptr connection_in, PASSTHRU_MSG& to_send, std::shared_ptr filter -) : MessageTx(connection_in, to_send), filter(filter), frames_sent(0), consumed_count(0) { +) : MessageTx(connection_in, to_send), filter(filter), frames_sent(0), consumed_count(0), txInFlight(FALSE), sendAll(FALSE), block_size(0) { CANid = ((uint8_t)fullmsg.Data[0]) << 24 | ((uint8_t)fullmsg.Data[1]) << 16 | ((uint8_t)fullmsg.Data[2]) << 8 | ((uint8_t)fullmsg.Data[3]); @@ -53,6 +53,10 @@ unsigned int MessageTx_ISO15765::addressLength() { BOOL MessageTx_ISO15765::sendNextFrame() { if (this->frames_sent >= this->framePayloads.size()) return FALSE; + if (block_size == 0 && !sendAll && this->frames_sent > 0)return FALSE; + if (block_size > 0 && !sendAll) block_size--; + + txInFlight = TRUE; if (auto conn_sp = this->connection.lock()) { if (auto panda_dev_sp = conn_sp->getPandaDev()) { @@ -68,41 +72,49 @@ BOOL MessageTx_ISO15765::sendNextFrame() { return TRUE; } +//Returns TRUE if receipt is consumed by the msg, FALSE otherwise. BOOL MessageTx_ISO15765::checkTxReceipt(J2534Frame frame) { - if (frames_sent < 1) return FALSE; + if (!txInFlight) return FALSE; if (frame.Data.size() >= addressLength() + 1 && (frame.Data[addressLength()] & 0xF0) == FRAME_FLOWCTRL) return FALSE; - if (frame.Data == fullmsg.Data.substr(0, 4) + framePayloads[frames_sent-1]) { + if (frame.Data == fullmsg.Data.substr(0, 4) + framePayloads[frames_sent - 1]) { //Check receipt is expected + txInFlight = FALSE; //Received the expected receipt. Allow another msg to be sent. - if (auto conn_sp = std::static_pointer_cast(this->connection.lock())) { - BOOL sendTxDone = (frames_sent == framePayloads.size()); - BOOL sendEcho = sendTxDone && conn_sp->loopback; - unsigned long flags = (filter == nullptr) ? fullmsg.TxFlags : this->filter->flags; + if (frames_sent == framePayloads.size()) { //Check message done + if (auto conn_sp = std::static_pointer_cast(this->connection.lock())) { + unsigned long flags = (filter == nullptr) ? fullmsg.TxFlags : this->filter->flags; - if (sendTxDone) { J2534Frame outframe(ISO15765); outframe.Timestamp = frame.Timestamp; outframe.RxStatus = TX_INDICATION | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID)); outframe.Data = frame.Data.substr(0, addressLength()); - conn_sp->addMsgToRxQueue(outframe); - } - if (sendEcho) { - J2534Frame outframe(ISO15765); - outframe.Timestamp = frame.Timestamp; - outframe.RxStatus = TX_MSG_TYPE | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID)); - outframe.Data = this->fullmsg.Data; - conn_sp->addMsgToRxQueue(outframe); - } - - } //TODO what if fails + if (conn_sp->loopback) { + J2534Frame outframe(ISO15765); + outframe.Timestamp = frame.Timestamp; + outframe.RxStatus = TX_MSG_TYPE | (flags & (ISO15765_ADDR_TYPE | CAN_29BIT_ID)); + outframe.Data = this->fullmsg.Data; + conn_sp->addMsgToRxQueue(outframe); + } + } //TODO what if fails + } return TRUE; } return FALSE; } BOOL MessageTx_ISO15765::isFinished() { - return 0; + return this->frames_sent == this->framePayloads.size() && !txInFlight; +} + +BOOL MessageTx_ISO15765::txReady() { + return block_size > 0 || sendAll || this->frames_sent == 0; +} + +void MessageTx_ISO15765::tx_flowcontrol(uint8_t block_size, std::chrono::microseconds separation_time, BOOL sendAll) { + this->block_size = block_size; + this->separation_time = separation_time; + this->sendAll = sendAll; } diff --git a/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.h b/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.h index 13c4d814c..1642202b9 100644 --- a/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.h +++ b/drivers/windows/pandaJ2534DLL/MessageTx_ISO15765.h @@ -22,13 +22,19 @@ public: virtual BOOL isFinished(); + virtual BOOL txReady(); + + void tx_flowcontrol(uint8_t block_size, std::chrono::microseconds separation_time, BOOL sendAll = FALSE); + std::shared_ptr filter; unsigned long frames_sent; unsigned long consumed_count; - //uint8_t block_size; + uint8_t block_size; unsigned long CANid; std::string data_prefix; std::string payload; BOOL isMultipart; std::vector framePayloads; + BOOL txInFlight; + BOOL sendAll; }; diff --git a/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp b/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp index 8bce281ee..731fa860c 100644 --- a/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp +++ b/drivers/windows/pandaJ2534DLL/PandaJ2534Device.cpp @@ -7,7 +7,7 @@ SCHEDULED_TX_MSG::SCHEDULED_TX_MSG(std::shared_ptr msgtx) : msgtx(msg } void SCHEDULED_TX_MSG::refreshExpiration() { - //expire += separation_time; + expire += this->msgtx->separation_time; } @@ -90,21 +90,52 @@ DWORD PandaJ2534Device::can_recv_thread() { if (msg_in.is_receipt) { synchronized(active_flow_control_txs_lock) { if (txMsgsAwaitingEcho.size() > 0) { - auto& msgtx = txMsgsAwaitingEcho.front()->msgtx; + auto msgtx = txMsgsAwaitingEcho.front()->msgtx; if (auto conn = msgtx->connection.lock()) { if (conn->isProtoCan() && conn->getPort() == msg_in.bus) { if (msgtx->checkTxReceipt(msg_out)) { - conn->processMessageReceipt(msg_out); - txMsgsAwaitingEcho.pop(); //nextWaitingMsg no longer valid + //Things to check: + // Frame not for this msg: Drop frame and alert. Error? + // Frame is for this msg, more tx frames required after a FC frame: Wait for FC frame to come and trigger next tx. + // Frame is for this msg, more tx frames required: Schedule next tx frame. + // Frame is for this msg, and is the final frame of the msg: Let conn process full msg, If another msg from this conn is available, register it. + auto tx_schedule = std::move(txMsgsAwaitingEcho.front()); + txMsgsAwaitingEcho.pop(); //Remove the TX object and schedule record. + + if (msgtx->isFinished()) { + conn->processMessageReceipt(msg_out); //Alert the connection a tx is done. + conn->txbuff.pop(); //Remove the finished TX message from the connection tx queue. + + //Remove the connection from the active list if it has no more scheduled TX msgs. + if (conn->txbuff.size() == 0) { + //Update records showing the connection no longer has a tx record scheduled. + this->ConnTxSet.erase(conn); + } else { + //Add the next scheduled tx from this conn + auto fcwrite = std::make_unique(conn->txbuff.back()); + this->insertMultiPartTxInQueue(std::move(fcwrite)); + } + + } else { + if (msgtx->txReady()) { //Not finished, ready to send next frame. + tx_schedule->refreshExpiration(); + insertMultiPartTxInQueue(std::move(tx_schedule)); + } else { + //Not finished, but next frame not ready (maybe waiting for flow control). + //Do not schedule more messages from this connection. + this->ConnTxSet.erase(conn); + } + } } } } else { - txMsgsAwaitingEcho.pop(); //nextWaitingMsg no longer valid + //Connection has died. Clear out the tx entry from device records. + txMsgsAwaitingEcho.pop(); + this->ConnTxSet.erase(conn); //connection is already dead, no need to schedule future tx msgs. } } } } else { - // TODO: Make this more efficient for (auto& conn : this->connections) if (conn->isProtoCan() && conn->getPort() == msg_in.bus) conn->processMessage(msg_out); @@ -138,10 +169,10 @@ DWORD PandaJ2534Device::msg_tx_thread() { goto break_flow_ctrl_loop; } if (std::chrono::steady_clock::now() >= this->active_flow_control_txs.front()->expire) { - auto fcontrol_write_real = std::move(this->active_flow_control_txs.front()); + auto scheduled_msg = std::move(this->active_flow_control_txs.front()); //Get the scheduled tx record. this->active_flow_control_txs.pop_front(); - if (fcontrol_write_real->msgtx->sendNextFrame()) - txMsgsAwaitingEcho.push(std::move(fcontrol_write_real)); + if (scheduled_msg->msgtx->sendNextFrame()) + txMsgsAwaitingEcho.push(std::move(scheduled_msg)); } else { //Ran out of things that need to be sent now. Sleep! auto time_diff = std::chrono::duration_cast (this->active_flow_control_txs.front()->expire - std::chrono::steady_clock::now()); @@ -170,8 +201,6 @@ void PandaJ2534Device::registerConnectionTx(std::shared_ptr con synchronized(ConnTxMutex) { auto ret = this->ConnTxSet.insert(conn); if (ret.second == FALSE) return; //Conn already exists. - this->ConnTxQueue.push(conn); - this->txInProgress = TRUE; auto fcwrite = std::make_unique(conn->txbuff.back()); this->insertMultiPartTxInQueue(std::move(fcwrite));