Skip to content

Commit

Permalink
Merge 265f7f5 into 120ce75
Browse files Browse the repository at this point in the history
  • Loading branch information
@GenoJAFord
GenoJAFord committed Jun 5, 2020
2 parents 120ce75 + 265f7f5 commit acd5fe3
Show file tree
Hide file tree
Showing 8 changed files with 219 additions and 20 deletions.
176 changes: 159 additions & 17 deletions src/diagnostics.cpp
View file
Expand Up @@ -36,10 +36,10 @@ namespace obd2 = openxc::diagnostics::obd2;

// receiveCanMessage - called from vi_firmware every time a CanMessage
// is QUEUE_POP from the busses' receiveQueue
// PERFORM_MULTIFRAME 0 The Old way Before 2020 (no multiframe messages)
// PERFORM_MULTIFRAME 1 Multi-frame stitched message feature
// MULTIFRAME 0 The Old way Before 2020 (no multiframe messages)
// MULTIFRAME 1 Multi-frame stitched message feature
//
#define PERFORM_MULTIFRAME 1
#define MULTIFRAME 1

static bool timedOut(ActiveDiagnosticRequest* request) {
// don't use staggered start with the timeout clock
Expand Down Expand Up @@ -159,7 +159,6 @@ static bool sendDiagnosticCanMessageBus2(

void openxc::diagnostics::reset(DiagnosticsManager* manager) {
if(manager->initialized) {
debug("Clearing existing diagnostic requests");
cleanupActiveRequests(manager, true);
}

Expand All @@ -171,8 +170,6 @@ void openxc::diagnostics::reset(DiagnosticsManager* manager) {
LIST_INSERT_HEAD(&manager->freeRequestEntries,
&manager->requestListEntries[i], listEntries);
}

debug("Reset diagnostics requests");
}

void openxc::diagnostics::initialize(DiagnosticsManager* manager, CanBus* buses,
Expand All @@ -191,7 +188,6 @@ void openxc::diagnostics::initialize(DiagnosticsManager* manager, CanBus* buses,

manager->obd2Bus = lookupBus(obd2BusAddress, buses, busCount);
obd2::initialize(manager);
debug("Initialized diagnostics");
}

static inline bool conflicting(ActiveDiagnosticRequest* request,
Expand Down Expand Up @@ -298,6 +294,7 @@ static openxc_VehicleMessage wrapDiagnosticResponseWithSabot(CanBus* bus,
return message;
}

#if (MULTIFRAME != 0)
const int MAX_MULTI_FRAME_MESSAGE_SIZE = 300;

static void sendPartialMessage(long timestamp,
Expand Down Expand Up @@ -345,16 +342,13 @@ static void sendPartialMessage(long timestamp,
",\"payload\":\"0x");

for(int index=0; (index<payload_size) && (numWritten < MAX_MULTI_FRAME_MESSAGE_SIZE); index++) {
messageBuffer[numWritten++]=((payload[index]>>4) > 9) ? (payload[index]>>4) + 'a' - 10 : (payload[index]>>4) + '0';
messageBuffer[numWritten++]=(((payload[index]>>4) & 0x0f) > 9) ? ((payload[index]>>4) & 0x0f) + 'a' - 10 : ((payload[index]>>4) & 0x0f) + '0';
messageBuffer[numWritten++]=((payload[index]&0xf) > 9) ? (payload[index]&0x0f) + 'a' - 10 : (payload[index]&0xf) + '0';
}
numWritten += snprintf(messageBuffer+numWritten,
MAX_MULTI_FRAME_MESSAGE_SIZE-numWritten,
"\"}");

debug("Before sendMessage in sendPartialMessage");
debug(messageBuffer);

int messageLen = strlen(messageBuffer) +1;
pipeline::sendMessage(pipeline,
(uint8_t*)messageBuffer, messageLen, MessageClass::SIMPLE);
Expand Down Expand Up @@ -393,6 +387,7 @@ static void relayPartialFrame(DiagnosticsManager* manager, // Only need for the
request->callback(manager, request, response, diagnostic_payload_to_integer(response));
}
}
#endif

static void relayDiagnosticResponse(DiagnosticsManager* manager,
ActiveDiagnosticRequest* request,
Expand Down Expand Up @@ -458,16 +453,15 @@ static void receiveCanMessage(DiagnosticsManager* manager,
&entry->handle, message->id, message->data, message->length);

if (response.multi_frame) {
#if (PERFORM_MULTIFRAME == 0)
if (0==1) // Do not call next but still keep reference
#else
#if (MULTIFRAME != 0)
relayPartialFrame(manager, entry, &response, pipeline);
#endif
if (!response.completed) {
time::tick(&entry->timeoutClock);
} else {
#if (PERFORM_MULTIFRAME == 0)
// This is the OLD Way of sending a Diagnostic Response
#if (MULTIFRAME == 0)
// This is the pre 2020 Way of sending a Diagnostic Response
// (all at once)
relayDiagnosticResponse(manager, entry, &response, pipeline);
#endif
}
Expand Down Expand Up @@ -860,6 +854,80 @@ bool openxc::diagnostics::isSupportedPID(int requestMode, int requestPID)
return false;
}

bool openxc::diagnostics::isStitchPID(int requestMode, int requestPID)
{
#if (MULTIFRAME==1)
if ((requestMode == 0x22) &&
((requestPID == 0xde00) || (requestPID == 0xde01))) {
return true;
}
#endif
return false;
}

bool openxc::diagnostics::generateAndSendEmulatedStitchMessages(int requestMode, int requestPID, Pipeline* pipeline) {
#if (MULTIFRAME!=1)
return false;
#else

if (requestPID == 0xde00) {
char emulPayload[] = {(char)0x62, (char)0xDE, (char)0x00,
(char)0x22, (char)0x2a, (char)0x04};
emulPayload[4] = rand() % 256;
emulPayload[5] = rand() % 256;
sendPartialMessage(0x00000000, // long timestamp,
-1, // int frame,
0x7d8, // int message_id,
0, // int bus,
0, // int total_size,
0x22, // int mode,
0xde00, // int pid,
0, // int value,
0, // int negative_response_code,
emulPayload, // const char *payload,
sizeof(emulPayload), // int payload_size,
pipeline);
} else {
char emulPayload1[] = {(char)0x62, (char)0xDE, (char)0x00,
(char)0x22, (char)0x2a, (char)0x04};
char emulPayload2[] = {(char)0x03, (char)0x00, (char)0x0a,
(char)0xc8, (char)0x00, (char)0x00,
(char)0x8a};
emulPayload1[5] = rand() % 256;
for(int cnt=0; cnt<7; cnt++) {
emulPayload2[cnt] = rand() % 256;
}

sendPartialMessage(0x00000000, // long timestamp,
0, // int frame, (First Frame)
0x7d8, // int message_id,
0, // int bus,
0, // int total_size,
0x22, // int mode,
0xde00, // int pid,
0, // int value,
0, // int negative_response_code,
emulPayload1, // const char *payload,
sizeof(emulPayload1), // int payload_size,
pipeline);

sendPartialMessage(0x00000000, // long timestamp,
-1, // int frame, (Last Frame)
0x7d8, // int message_id,
0, // int bus,
0, // int total_size,
0x22, // int mode,
0xde00, // int pid,
0, // int value,
0, // int negative_response_code,
emulPayload2, // const char *payload,
sizeof(emulPayload2), // int payload_size,
pipeline);
}
return true;
#endif
}

void openxc::diagnostics::generateEmulatorPayload(openxc_VehicleMessage* vehicleMessage, bool isSuccess)
{
vehicleMessage->diagnostic_response.success = isSuccess;
Expand All @@ -876,6 +944,72 @@ void openxc::diagnostics::generateEmulatorPayload(openxc_VehicleMessage* vehicle
}
}

bool openxc::diagnostics::isVINPid(int requestMode, int requestPID)
{
#if (MULTIFRAME==1)
if ((requestMode == 9) && (requestPID == 2)) {
return true;
}
#endif
return false;
}

static const char *VINArray[] = {
"1FDWX3C60C0012685", // 2012 Ford
"2FTDX15G0C0010824", // 1982 Ford F150
"2FDHF37M0R0011608", // 1994 Ford F350
"1FTNX20L71EC79927", // 2001 Ford F250 Super Duty
"1FTFX1CF2DFC76209", // 2013 Ford F150
"1FDXF46P37EB40030", // 2007 Ford F450
"1FTBR10T9GUA26470", // 1986 Ford Ranger
"1FTNS2ELXADB03208", // 2010 Ford E Series Van
"JH4KA3150HC004866", // 1987 Acura Legend
"1G8MC35B38Y119771", // 2008 Saturn Sky
"1ZVBP8CH7A5121324", // 2020 Ford Mustang
"WD5WD641525381291", // 2002 Freightliner Sprinter
"WBANF73576CG65408", // 2006 BMW 5 Series
"1FTEX14H0RKA51281", // 1994 Ford F150
"1GNEL19X73B130926", // 2003 Chevrolet Astro
"1C6RD6KT4CS332867", // 2012 Dodge Ram 1500
"3D4GG47B09T581222", // 2009 Dodge Journey
"JH4DB1670MS000448", // 1991 Acura Integra
"1GNDT13W5R2133070", // 1994 Chevrolet S10 Blazer
"1G6CD1184H4323745" // 1987 Cadillac Deville
};

bool openxc::diagnostics::generateAndSendVINStitchMessages(int messageId, int requestMode, int requestPID, Pipeline* pipeline) {
#if (MULTIFRAME!=1)
return false;
#endif

int sampleSize = sizeof(VINArray) / sizeof(VINArray[0]);
int selection = rand() % sampleSize;
const int packetSize = 4; // Send max "packetSize" ascii per message
int numPackets = (strlen(VINArray[selection]) + packetSize - 1) / packetSize;

int index = 0;
for(int count=0; count<numPackets; count++) {
int remaining = strlen(VINArray[selection]) - index;
int payloadSize = (packetSize < remaining) ? packetSize : remaining;
int frame = (count == numPackets - 1) ? -1 : count;
sendPartialMessage(0x00000000, // long timestamp,
frame, // int frame,
messageId + 8, // int message_id,
0, // int bus,
0, // int total_size,
requestMode, // int mode,
requestPID, // int pid,
0, // int value,
0, // int negative_response_code,
&VINArray[selection][index], // const char *payload,
payloadSize, // int payload_size,
pipeline);
index += packetSize;
}

return true;
}

bool openxc::diagnostics::handleDiagnosticCommand(DiagnosticsManager* manager, openxc_ControlCommand* command)
{
bool status = true;
Expand Down Expand Up @@ -910,7 +1044,15 @@ bool openxc::diagnostics::handleDiagnosticCommand(DiagnosticsManager* manager, o
{
message.diagnostic_response.mode = commandRequest->mode;

if (isSupportedPID(commandRequest->mode, commandRequest->pid))
if (isStitchPID(commandRequest->mode, commandRequest->pid)) {
generateAndSendEmulatedStitchMessages(commandRequest->mode, commandRequest->pid,
&getConfiguration()->pipeline);
} else if (isVINPid(commandRequest->mode, commandRequest->pid)) {
generateAndSendVINStitchMessages(commandRequest->message_id,
commandRequest->mode, commandRequest->pid,
&getConfiguration()->pipeline);
}
else if (isSupportedPID(commandRequest->mode, commandRequest->pid))
{
message.diagnostic_response.pid = commandRequest->pid;

Expand Down
11 changes: 11 additions & 0 deletions src/diagnostics.h
View file
Expand Up @@ -339,8 +339,19 @@ bool isSupportedMode(int requestMode);

bool isSupportedPID(int requestMode, int requestPID);

bool isStitchPID(int requestMode, int requestPID);

bool generateAndSendEmulatedStitchMessages(int requestMode, int requestPID, openxc::pipeline::Pipeline* pipeline);

void generateEmulatorPayload(openxc_VehicleMessage* vehicleMessage, bool isSuccess);

bool isVINPid(int requestMode, int requestPID);

//const char *VINArray[];

bool generateAndSendVINStitchMessages(int messageId, int requestMode, int requestPID, openxc::pipeline::Pipeline* pipeline);


/* Public: Handle an incoming command that claims to be a diagnostic request.
*
* This handles requests in the OpenXC message format
Expand Down
32 changes: 32 additions & 0 deletions src/payload/payload.cpp
View file
Expand Up @@ -3,6 +3,7 @@
#include "payload/protobuf.h"
#include "payload/messagepack.h"
#include "util/log.h"
#include <stdio.h>

namespace payload = openxc::payload;

Expand All @@ -29,13 +30,44 @@ openxc_DynamicField openxc::payload::wrapBoolean(bool value) {
return sabot;
}

// Diagnostically print out the hex values in the payload
void dumpPayload(unsigned char *payload, size_t length) {
int finished = 0;
size_t offset = 0;
const size_t MAX = 12;
while(!finished) {
char buf[26];
buf[0] = 0;
size_t l = length-offset;
if (l > MAX)
l = MAX;
for(size_t i=0; i<l; i++) {
buf[i*2]= ((payload[i+offset]>>4) > 9) ? (payload[i+offset]>>4) + 'A' - 10 : (payload[i+offset]>>4) + '0';
buf[i*2+1]=((payload[i+offset]&0xf) > 9) ? (payload[i+offset]&0x0f) + 'A' - 10 : (payload[i+offset]&0xf) + '0';
buf[i*2+2]=0;
}
debug(buf);
offset += MAX;
if (offset >= length) finished = 1;
}
}

static void dumpNum(int value) {
char buf[10];
sprintf(buf,"%d",value);
debug(buf);
}

size_t openxc::payload::deserialize(uint8_t payload[], size_t length,
PayloadFormat format, openxc_VehicleMessage* message) {
size_t bytesRead = 0;
if(format == PayloadFormat::JSON) {
bytesRead = payload::json::deserialize(payload, length, message);
} else if(format == PayloadFormat::PROTOBUF) {
bytesRead = payload::protobuf::deserialize(payload, length, message);
debug("deserialize protobuf");
dumpNum(bytesRead);
dumpPayload(payload, bytesRead);
} else if(format == PayloadFormat::MESSAGEPACK){
bytesRead = payload::messagepack::deserialize(payload, length, message);
} else {
Expand Down
6 changes: 5 additions & 1 deletion src/payload/protobuf.cpp
View file
Expand Up @@ -6,12 +6,15 @@

using openxc::util::log::debug;

extern void dumpPayload(unsigned char *payload, size_t length);

size_t openxc::payload::protobuf::deserialize(uint8_t payload[], size_t length,
openxc_VehicleMessage* message) {
pb_istream_t stream = pb_istream_from_buffer(payload, length);
if(!pb_decode_delimited(&stream, openxc_VehicleMessage_fields, message)) {
debug("Protobuf decoding failed with %s", PB_GET_ERROR(&stream));
return 0;
dumpPayload(payload, length);
//return 0; // gja commented out since it was not clearing queue and resulting in overflowed queue and crash
}
return length - stream.bytes_left;
}
Expand All @@ -28,5 +31,6 @@ int openxc::payload::protobuf::serialize(openxc_VehicleMessage* message,
message)) {
debug("Error encoding protobuf: %s", PB_GET_ERROR(&stream));
}

return stream.bytes_written;
}
2 changes: 1 addition & 1 deletion src/platform/pic32/server_task.cpp
View file
Expand Up @@ -307,4 +307,4 @@ void openxc::server_task::commandCheck(TelitDevice* device) {

return;

}
}
1 change: 0 additions & 1 deletion src/platform/pic32/usb.cpp
View file
Expand Up @@ -212,7 +212,6 @@ void openxc::interface::usb::deinitialize(UsbDevice* usbDevice) {

void openxc::interface::usb::read(UsbDevice* device, UsbEndpoint* endpoint,
util::bytebuffer::IncomingMessageCallback callback) {

#ifdef FS_SUPPORT
if(fs::getmode() == FS_STATE::USB_CONNECTED){
MSDActivity();
Expand Down
7 changes: 7 additions & 0 deletions src/util/log.cpp
View file
Expand Up @@ -33,3 +33,10 @@ void openxc::util::log::debug(const char* format, ...) {
va_end(args);
#endif // __DEBUG__
}

extern "C" {
void c_debug(char *str) {
openxc::util::log::debug(str);
}
}

4 changes: 4 additions & 0 deletions src/util/log.h
View file
Expand Up @@ -30,4 +30,8 @@ void debugUart(const char* message);
} // namespace util
} // namespace openxc

extern "C" {
void c_debug(char *str);
}

#endif // _LOG_H_

0 comments on commit acd5fe3

Please sign in to comment.