int register_UUID(int sock, struct sockaddr_in *si_remote)
{
MSG_T msg;
memset(&msg, 0, sizeof(MSG_T));
msg.event = REGISTER;
msg.SRC_UUID = g_UUID;
sendRequest(sock, si_remote, &msg);
return waitMsg(sock, si_remote);
}
/*Main Loop*/
void ram(void)
{
initDisplay();
uint8_t key;
do
{
key = getInput();
if(key == BTN_DOWN)
waitMsg();
else if (key == BTN_UP)
initMesh();
} while(key != BTN_ENTER);
}
void * liftCtrlCallback(){
struct message msg;
int i;
while (1) {
waitMsg(MSG_TYPE_CTRL_CALLBACK);
//printf("[LIFTCTRL_CALLBACK] msg: %d(%f)\n", buf.command.type, buf.command.floor);
int code = buf.command.type;
switch(code){
case LIFT_UP:
case LIFT_DOWN:
for (i = 0; i < num_clients; i++){
if (client_array[i].clientType == CLIENT_ELEVATOR){
write(client_array[i].sockfd, &(buf.command), sizeof(struct message));
} else if (client_array[i].clientType == CLIENT_FLOOR){
msg = buf.command;
if ((int)msg.floor == (int)client_array[i].clientInfo){
msg.type = ELEVATOR_APPEAR;
} else {
msg.type = ELEVATOR_MOVING;
}
write(client_array[i].sockfd, &msg, sizeof(struct message));
}
}
break;
case LIFT_STOP:
for (i = 0; i < num_clients; i++){
if (client_array[i].clientType == CLIENT_ELEVATOR){
write(client_array[i].sockfd, &(buf.command), sizeof(struct message));
} else if (client_array[i].clientType == CLIENT_FLOOR){
msg = buf.command;
if ((int)msg.floor == (int)client_array[i].clientInfo){
msg.type = ELEVATOR_APPEAR;
write(client_array[i].sockfd, &msg, sizeof(struct message));
}
msg.type = ELEVATOR_STOP;
write(client_array[i].sockfd, &msg, sizeof(struct message));
}
}
break;
default:
break;
}
}
return NULL;
}
void obp_canraw(void *pvParameters)
{
//>>>> oobdtemple protocol initmain >>>>
int keeprunning = 1;
data_packet *dp;
data_packet actDataPacket;
UBaseType_t busToUse = *(UBaseType_t *) pvParameters;
/* function pointers to the bus interface */
extern bus_init actBus_init;
extern bus_send actBus_send;
extern bus_flush actBus_flush;
extern bus_param actBus_param;
extern bus_close actBus_close;
extern QueueHandle_t protocolQueue;
extern QueueHandle_t outputQueue;
extern QueueHandle_t inputQueue;
MsgData *msg;
MsgData *ownMsg;
param_data *args;
extern SemaphoreHandle_t protocollBinarySemaphore;
UBaseType_t msgType;
UBaseType_t timeout = 0;
int i;
//catch the "Protocoll is running" Semaphore
xSemaphoreTake(protocollBinarySemaphore, portMAX_DELAY);
/* activate the bus... */
odbarr[busToUse] ();
actBus_init();
ODPBuffer *protocolBuffer;
protocolBuffer = NULL;
// start with the protocol specific initalisation
//<<<< oobdtemple protocol initmain <<<<
extern print_cbf printdata_CAN;
UBaseType_t stateMachine_state = 0;
UBaseType_t actBufferPos = 0;
/* tell the Rx-ISR about the function to use for received data */
busControl(ODB_CMD_RECV, odp_canraw_recvdata);
protocolBuffer = createODPBuffer(CANRAWBUFFERSIZE);
if (protocolBuffer == NULL) {
keeprunning = 0;
} else {
protocolBuffer->len = 0;
}
extern protocolConfigPtr actProtConfigPtr;
struct CanRawConfig *protocolConfig;
protocolConfig = pvPortMalloc(sizeof(struct CanRawConfig));
if (protocolConfig == NULL) {
keeprunning = 0;
} else {
actProtConfigPtr = protocolConfig;
protocolConfig->recvID = 0x7DF;
protocolConfig->separationTime = 0;
protocolConfig->showBusTransfer = 0;
}
//>>>> oobdtemple protocol mainloop_start >>>>
for (; keeprunning;) {
if (MSG_NONE != (msgType = waitMsg(protocolQueue, &msg, portMAX_DELAY))) // portMAX_DELAY
/* handle message */
{
switch (msgType) {
//<<<< oobdtemple protocol mainloop_start <<<<
//>>>> oobdtemple protocol MSG_BUS_RECV >>>>
case MSG_BUS_RECV:
dp = msg->addr;
//<<<< oobdtemple protocol MSG_BUS_RECV <<<<
if (protocolConfig->showBusTransfer > 0) {
odp_canraw_dumpFrame(dp, printdata_CAN);
}
// no more action, Raw CAN does not manage any answers from the bus
//>>>> oobdtemple protocol MSG_SERIAL_DATA >>>>
break;
case MSG_SERIAL_DATA:
//<<<< oobdtemple protocol MSG_SERIAL_DATA <<<<
if (stateMachine_state == SM_CANRAW_STANDBY) { /* only if just nothing to do */
dp = (data_packet *) msg->addr;
// data block received from serial input which need to be handled now
if (((protocolBuffer->len) + dp->len) <=
CANRAWBUFFERSIZE) {
/* copy the data into the uds- buffer */
for (i = 0; i < dp->len; i++) {
protocolBuffer->data[protocolBuffer->len++] =
dp->data[i];
}
} else {
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_CANRAW_DATA_TOO_LONG_ERR,
(protocolBuffer->len) + dp->len,
ERR_CODE_CANRAW_DATA_TOO_LONG_ERR_TEXT);
}
}
//>>>> oobdtemple protocol MSG_SERIAL_PARAM_1 >>>>
break;
case MSG_SERIAL_PARAM:
args = (UBaseType_t *) msg->addr;
/*
* DEBUGPRINT("protocol parameter received %ld %ld %ld\n",
args->args[ARG_RECV], args->args[ARG_CMD],
args->args[ARG_VALUE_1]);
*/
switch (args->args[ARG_RECV]) {
case FBID_PROTOCOL_GENERIC:
/*
* DEBUGPRINT
("generic protocol parameter received %ld %ld\n",
args->args[ARG_CMD], args->args[ARG_VALUE_1]);
*/
switch (args->args[ARG_CMD]) {
case PARAM_INFO:
//<<<< oobdtemple protocol MSG_SERIAL_PARAM_1 <<<<
CreateParamOutputMsg(args, odp_canraw_printParam);
//>>>> oobdtemple protocol MSG_SERIAL_PARAM_2 >>>>
break;
// and here we proceed all command parameters
case PARAM_LISTEN:
xTickCurrent = 0; // set current Timestamp to "0" if Listen mode ist activated
protocolConfig->showBusTransfer =
args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_GENERIC,
ERR_CODE_NO_ERR, 0, NULL);
break;
default:
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_OS_UNKNOWN_COMMAND, 0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
break;
//<<<< oobdtemple protocol MSG_SERIAL_PARAM_2 <<<<
case FBID_PROTOCOL_SPEC:
//DEBUGPRINT ("can raw protocol parameter received %ld %ld\n", args->args[ARG_CMD], args->args[ARG_VALUE_1]);
switch (args->args[ARG_CMD]) {
// first we commend out all parameters which are not used to generate the right "unknown parameter" message in the default - area
/*
case PARAM_ECHO:
break;
case PARAM_TIMEOUT_PENDING:
break;
case PARAM_BLOCKSIZE:
break;
*/
case PARAM_CANRAW_FRAME_DELAY:
protocolConfig->separationTime =
args->args[ARG_VALUE_1] + 1;
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_CANRAW_SENDID:
protocolConfig->recvID = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
default:
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_OS_UNKNOWN_COMMAND,
0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
break;
//>>>> oobdtemple protocol MSG_OTHERS >>>>
case FBID_BUS_GENERIC:
case FBID_BUS_SPEC:
actBus_param(args); /* forward the received params to the underlying bus. */
break;
default:
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_OS_UNKNOWN_COMMAND,
0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
//<<<< oobdtemple protocol MSG_OTHERS <<<<
//>>>> oobdtemple protocol MSG_INIT >>>>
case MSG_INIT:
if (protocolBuffer != NULL) {
protocolBuffer->len = 0;
}
//<<<< oobdtemple protocol MSG_INIT <<<<
//>>>> oobdtemple protocol MSG_PROTOCOL_STOP >>>>
break;
case MSG_PROTOCOL_STOP:
keeprunning = 0;
break;
//<<<< oobdtemple protocol MSG_PROTOCOL_STOP <<<<
//>>>> oobdtemple protocol MSG_SEND_BUFFER >>>>
case MSG_SEND_BUFFER:
/* let's Dance: Starting the transfer protocol */
//<<<< oobdtemple protocol MSG_SEND_BUFFER <<<<
if (protocolBuffer->len > 0) {
actBufferPos = 0;
for (; sendMoreFrames(protocolBuffer, &actBufferPos, &protocolConfig->showBusTransfer, &stateMachine_state, &timeout, printdata_CAN, actBus_send);); // fire all in one shot.
//>>>> oobdtemple protocol MSG_SEND_BUFFER_2 >>>>
} else { /* no data to send? */
createCommandResultMsg
(FBID_PROTOCOL_GENERIC, ERR_CODE_NO_ERR, 0, NULL);
/* just release the input again */
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE, inputQueue, NULL)) {
printser_string("Input queue is full!");
DEBUGPRINT
("FATAL ERROR: input queue is full!\n", 'a');
}
}
break;
//<<<< oobdtemple protocol MSG_SEND_BUFFER_2 <<<<
//>>>> oobdtemple protocol MSG_TICK >>>>
case MSG_TICK:
//<<<< oobdtemple protocol MSG_TICK <<<<
if (timeout > 0) { /* we just waiting for the next frame to send */
if (timeout == 1) { /* time's gone... */
for (; sendMoreFrames(protocolBuffer, &actBufferPos, &protocolConfig->showBusTransfer, &stateMachine_state, &timeout, printdata_CAN, actBus_send);); // fire all in one shot.
if (timeout < 2) { //
protocolBuffer->len = 0;
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_NO_ERR, 0, NULL);
stateMachine_state = SM_CANRAW_STANDBY;
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE, inputQueue,
NULL)) {
printser_string("INPQUE_FULL");
DEBUGPRINT
("FATAL ERROR: input queue is full!\n",
'a');
}
}
}
timeout--;
}
//>>>> oobdtemple protocol final >>>>
break;
}
disposeMsg(msg);
}
/* vTaskDelay (5000 / portTICK_PERIOD_MS); */
}
/* Do all cleanup here to finish task */
actBus_close();
vPortFree(protocolConfig);
freeODPBuffer(protocolBuffer);
xSemaphoreGive(protocollBinarySemaphore);
vTaskDelete(NULL);
}
void obp_uds(void *pvParameters)
{
//>>>> oobdtemple protocol initmain >>>>
int keeprunning = 1;
data_packet *dp;
data_packet actDataPacket;
UBaseType_t busToUse = *(UBaseType_t *) pvParameters;
/* function pointers to the bus interface */
extern bus_init actBus_init;
extern bus_send actBus_send;
extern bus_flush actBus_flush;
extern bus_param actBus_param;
extern bus_close actBus_close;
extern QueueHandle_t protocolQueue;
extern QueueHandle_t outputQueue;
extern QueueHandle_t inputQueue;
MsgData *msg;
MsgData *ownMsg;
param_data *args;
extern SemaphoreHandle_t protocollBinarySemaphore;
UBaseType_t msgType;
UBaseType_t timeout = 0;
UBaseType_t showBusTransfer = 0;
int i;
//catch the "Protocoll is running" Semaphore
xSemaphoreTake(protocollBinarySemaphore, portMAX_DELAY);
DEBUGPRINT("Start Bus nr %ld\n", busToUse);
/* activate the bus... */
odbarr[busToUse] ();
actBus_init();
ODPBuffer *protocolBuffer;
protocolBuffer = NULL;
// start with the protocol specific initalisation
//<<<< oobdtemple protocol initmain <<<<
extern print_cbf printdata_CAN;
UBaseType_t sequenceCounter;
UBaseType_t remainingBytes;
UBaseType_t actBufferPos;
UBaseType_t actFrameLen;
UBaseType_t separationTime_ST = 0;
UBaseType_t actBlockSize_BS = 0;
UBaseType_t actSeparationTime_STTicks = 0;
UBaseType_t stateMachine_state = 0;
unsigned char telegram[8];
struct TPElement *tpList = NULL; //!< keeps the list of testerPresents
/* tell the Rx-ISR about the function to use for received data */
busControl(ODB_CMD_RECV, odp_uds_recvdata);
protocolBuffer = createODPBuffer(UDSSIZE);
if (protocolBuffer == NULL) {
keeprunning = 0;
} else {
protocolBuffer->len = 0;
}
extern protocolConfigPtr actProtConfigPtr;
struct UdsConfig *protocolConfig;
protocolConfig = pvPortMalloc(sizeof(struct UdsConfig));
if (protocolConfig == NULL) {
keeprunning = 0;
} else {
actProtConfigPtr = protocolConfig;
/* Init default parameters */
protocolConfig->recvID = 0x7DF;
protocolConfig->sendID = 0x00; // 0 disables special sendID
protocolConfig->timeout = 6;
protocolConfig->timeoutPending = 150;
protocolConfig->blockSize = 0;
protocolConfig->separationTime = 0;
protocolConfig->tpFreq = 250;
protocolConfig->tpType = 0x80;
}
//>>>> oobdtemple protocol mainloop_start >>>>
for (; keeprunning;) {
if (MSG_NONE != (msgType = waitMsg(protocolQueue, &msg, portMAX_DELAY))) // portMAX_DELAY
/* handle message */
{
switch (msgType) {
//<<<< oobdtemple protocol mainloop_start <<<<
//>>>> oobdtemple protocol MSG_BUS_RECV >>>>
case MSG_BUS_RECV:
dp = msg->addr;
//<<<< oobdtemple protocol MSG_BUS_RECV <<<<
if (showBusTransfer > 0) {
odp_uds_dumpFrame(dp, printdata_CAN);
}
if (((protocolConfig->sendID == 0 ? dp->recv == (protocolConfig->recvID | 8) : dp->recv == protocolConfig->sendID)) || protocolConfig->recvID == 0x7DF) { /* Tester Address correct / we sendes a broadcast (protocolConfig->recvID==0x7DF)? */
if (dp->data[0] == 0x03 && dp->data[1] == 0x7f && dp->data[3] == 0x78) //Response pending
{
timeout = protocolConfig->timeoutPending;
} else {
if (stateMachine_state == SM_UDS_WAIT_FOR_FC) {
if ((dp->data[0] & 0xF0) == 0x30) { /* FlowControl */
DEBUGPRINT("FlowControl received\n", 'a');
/* as we now probably have to send many frames first before we receive any
new answer from the module, we have to disable the timeout as long as we've sent the last frame
*/
timeout = 0;
//! \todo how to correctly support "wait" if LowNibble of PCI is 1?
if (protocolConfig->blockSize == 0) {
actBlockSize_BS = dp->data[1]; /* take the block size out of the FC block */
} else {
actBlockSize_BS = protocolConfig->blockSize; /* use the config value instead the one from FC */
}
if (actBlockSize_BS > 0) {
actBlockSize_BS++;
DEBUGPRINT
("Blocksize received with %ld ticks\n",
actBlockSize_BS);
}
if (protocolConfig->separationTime == 0) {
separationTime_ST = dp->data[2]; /* take the separation time out of the FC block */
} else {
separationTime_ST = protocolConfig->separationTime; /* use the config value instead the one from FC */
}
if (separationTime_ST > 0) {
stateMachine_state =
SM_UDS_SLEEP_UNTIL_SINGLE_CF;
actSeparationTime_STTicks =
separationTime_ST /
portTICK_PERIOD_MS;
actSeparationTime_STTicks++;
if (actSeparationTime_STTicks < 2) {
actSeparationTime_STTicks = 2;
}
DEBUGPRINT
("FlowControl Delay received with %d ticks\n",
actSeparationTime_STTicks);
} else {
stateMachine_state = SM_UDS_SEND_CF;
}
} else { /* wrong answer */
stateMachine_state = SM_UDS_STANDBY;
protocolBuffer->len = 0;
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_UDS_MISSING_FLOW_CONTROL,
(dp->data[0] & 0xF0),
ERR_CODE_UDS_MISSING_FLOW_CONTROL_TEXT);
}
}
if (stateMachine_state == SM_UDS_SEND_CF) {
/* Caution: This "if state" needs to be straight after
the Flow Control handling above, so that when the state
SM_UDS_SEND_CF is reached, the state machine starts straight to send
*/
//! \todo delayed, block wise sending of Consecutive frame still needs to be implemented
while (remainingBytes > 0) {
DEBUGPRINT("Remaining bytes: %ld\n",
remainingBytes);
actFrameLen =
remainingBytes >
7 ? 7 : remainingBytes;
odp_uds_data2CAN(&protocolBuffer->data
[actBufferPos],
&telegram[0], actFrameLen,
1);
sequenceCounter =
sequenceCounter <
15 ? sequenceCounter + 1 : 0;
actBufferPos += actFrameLen;
remainingBytes -= actFrameLen;
actDataPacket.data[0] = 0x20 + sequenceCounter; // prepare CF
if (showBusTransfer > 0) {
odp_uds_dumpFrame(&actDataPacket,
printdata_CAN);
}
actBus_send(&actDataPacket);
}
stateMachine_state = SM_UDS_WAIT_FOR_ANSWER;
timeout = protocolConfig->timeout;
}
if (stateMachine_state == SM_UDS_WAIT_FOR_CF) {
if ((dp->data[0] & 0xF0) == 0x20) { /* consecutive Frame */
DEBUGPRINT
("Consecutive Frame seq. %ld\n",
sequenceCounter);
sequenceCounter =
sequenceCounter >
14 ? 0 : sequenceCounter + 1;
if ((dp->data[0] & 0x0F) ==
sequenceCounter) {
DEBUGPRINT("Sequence ok seq. %ld\n",
sequenceCounter);
actFrameLen =
remainingBytes >
7 ? 7 : remainingBytes;
udp_uds_CAN2data(protocolBuffer,
&(dp->data[1]),
actBufferPos,
actFrameLen);
actBufferPos += actFrameLen;
remainingBytes -= actFrameLen;
timeout = protocolConfig->timeout;
DEBUGPRINT
("actualBufferPos %ld remaining Bytes %ld\n",
actBufferPos, remainingBytes);
if (remainingBytes == 0) { /* finished */
stateMachine_state =
SM_UDS_STANDBY;
timeout = 0;
/* to dump the buffer, we send the address of the udsbuffer to the print routine */
ownMsg =
createMsg(&protocolBuffer, 0);
/* add correct print routine; */
ownMsg->print =
odp_uds_printdata_Buffer;
// send event information to the ILM task
CreateEventMsg
(MSG_EVENT_PROTOCOL_RECEIVED,
0);
/* forward data to the output task */
if (pdPASS !=
sendMsg(MSG_DUMP_BUFFER,
outputQueue, ownMsg)) {
DEBUGPRINT
("FATAL ERROR: output queue is full!\n",
'a');
}
}
} else { /* sequence error! */
stateMachine_state = SM_UDS_STANDBY;
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_UDS_WRONG_SEQUENCE_COUNT,
(dp->data[0] & 0x0F),
ERR_CODE_UDS_WRONG_SEQUENCE_COUNT_TEXT);
DEBUGPRINT
("Sequence Error! Received %d , expected %ld\n",
dp->data[0] & 0x0F,
sequenceCounter);
timeout = 0;
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE,
inputQueue, NULL)) {
DEBUGPRINT
("FATAL ERROR: input queue is full!\n",
'a');
}
}
} else {
stateMachine_state = SM_UDS_STANDBY;
createCommandResultMsg
(FBID_PROTOCOL_GENERIC,
ERR_CODE_UDS_MISSING_FIRST_FRAME,
(dp->data[0] & 0xF0),
ERR_CODE_UDS_MISSING_FIRST_FRAME_TEXT);
DEBUGPRINT
("Wrong Frame Error! Received %d , expected 0x2x\n",
dp->data[0]);
timeout = 0;
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE,
inputQueue, NULL)) {
DEBUGPRINT
("FATAL ERROR: input queue is full!\n",
'a');
}
}
}
if (stateMachine_state == SM_UDS_WAIT_FOR_ANSWER) {
if ((dp->data[0] & 0xF0) == 0x10) { /* FirstFrame */
sequenceCounter = 0; //first Frame counts as sequence 0 already
remainingBytes =
(dp->data[0] & 0xF) * 256 +
dp->data[1];
actBufferPos = 6;
DEBUGPRINT("First Frame with %ld Bytes\n",
remainingBytes);
protocolBuffer->len = remainingBytes; /* set the buffer size alredy inhope, that all goes well ;-) */
remainingBytes -= 6; /* the first 6 bytes are already in the FF */
udp_uds_CAN2data(protocolBuffer,
&(dp->data[2]), 0, 6);
actDataPacket.recv =
protocolConfig->recvID;
actDataPacket.data = &telegram[0];
actDataPacket.len = 8;
for (i = 0; i < 8; i++) { /* just fill the telegram with 0 */
telegram[i] = 0;
}
telegram[0] = 0x30; /* 0x30 = 3=>FlowControl, 0=>CTS = ContinoueToSend */
stateMachine_state = SM_UDS_WAIT_FOR_CF;
timeout = protocolConfig->timeout;
if (showBusTransfer > 0) {
odp_uds_dumpFrame(&actDataPacket,
printdata_CAN);
}
actBus_send(&actDataPacket);
} else {
if ((dp->data[0] & 0xF0) == 0x00) { /*Single Frame */
DEBUGPRINT
("Single Frame with %d Bytes\n",
dp->data[0]);
protocolBuffer->len = dp->data[0];
udp_uds_CAN2data(protocolBuffer,
&(dp->data[1]), 0,
dp->data[0]);
stateMachine_state = SM_UDS_STANDBY;
timeout = 0;
/* to dump the buffer, we send the address of the udsbuffer to the print routine */
ownMsg =
createMsg(&protocolBuffer,
sizeof(protocolBuffer));
/* add correct print routine; */
ownMsg->print =
odp_uds_printdata_Buffer;
// send event information to the ILM task
CreateEventMsg
(MSG_EVENT_PROTOCOL_RECEIVED, 0);
/* forward data to the output task */
if (pdPASS !=
sendMsg(MSG_DUMP_BUFFER,
outputQueue, ownMsg)) {
DEBUGPRINT
("FATAL ERROR: output queue is full!\n",
'a');
}
}
}
}
}
}
//>>>> oobdtemple protocol MSG_SERIAL_DATA >>>>
break;
case MSG_SERIAL_DATA:
if (stateMachine_state == SM_UDS_STANDBY) { /* only if just nothing to do */
dp = (data_packet *) msg->addr;
// data block received from serial input which need to be handled now
//<<<< oobdtemple protocol MSG_SERIAL_DATA <<<<
if (((protocolBuffer->len) + dp->len) <= UDSSIZE) {
/* copy the data into the uds- buffer */
for (i = 0; i < dp->len; i++) {
protocolBuffer->data[protocolBuffer->len++] =
dp->data[i];
}
} else {
createCommandResultMsg(FBID_PROTOCOL_GENERIC,
ERR_CODE_UDS_DATA_TOO_LONG_ERR,
(protocolBuffer->len) +
dp->len,
ERR_CODE_UDS_DATA_TOO_LONG_ERR_TEXT);
}
}
//>>>> oobdtemple protocol MSG_SERIAL_PARAM_1 >>>>
break;
case MSG_SERIAL_PARAM:
args = (UBaseType_t *) msg->addr;
DEBUGPRINT("protocol parameter received %ld %ld %ld\n",
args->args[ARG_RECV], args->args[ARG_CMD],
args->args[ARG_VALUE_1]);
switch (args->args[ARG_RECV]) {
case FBID_PROTOCOL_GENERIC:
DEBUGPRINT
("generic protocol parameter received %ld %ld\n",
args->args[ARG_CMD], args->args[ARG_VALUE_1]);
switch (args->args[ARG_CMD]) {
case PARAM_INFO:
//<<<< oobdtemple protocol MSG_SERIAL_PARAM_1 <<<<
CreateParamOutputMsg(args, odp_uds_printParam);
//>>>> oobdtemple protocol MSG_SERIAL_PARAM_2 >>>>
break;
// and here we proceed all command parameters
case PARAM_LISTEN:
showBusTransfer = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_GENERIC,
ERR_CODE_NO_ERR, 0, NULL);
break;
default:
createCommandResultMsg(FBID_PROTOCOL_GENERIC,
ERR_CODE_OS_UNKNOWN_COMMAND,
0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
break;
//<<<< oobdtemple protocol MSG_SERIAL_PARAM_2 <<<<
case FBID_PROTOCOL_SPEC:
DEBUGPRINT("uds protocol parameter received %ld %ld\n",
args->args[ARG_CMD],
args->args[ARG_VALUE_1]);
switch (args->args[ARG_CMD]) {
// first we commend out all parameters which are not used to generate the right "unknown parameter" message in the default - area
/*
case PARAM_ECHO:
break;
case PARAM_TIMEOUT_PENDING:
break;
case PARAM_BLOCKSIZE:
break;
case PARAM_FRAME_DELAY:
break;
*/
case PARAM_TIMEOUT:
protocolConfig->timeout =
args->args[ARG_VALUE_1] + 1;
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_RECVID:
protocolConfig->recvID = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_SENDID:
protocolConfig->sendID = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_TP_ON:
if (odp_uds_addTesterPresents(&tpList,
args->args
[ARG_VALUE_1],
protocolConfig->
tpFreq,
protocolConfig->
tpType)) {
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0,
NULL);
} else {
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_UDS_TP_OOM,
0,
ERR_CODE_UDS_TP_OOM_TEXT);
}
break;
case PARAM_TP_OFF:
odp_uds_deleteTesterPresents(&tpList,
args->args
[ARG_VALUE_1]);
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_TP_TYPE:
protocolConfig->tpType = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
case PARAM_TP_FREQ:
protocolConfig->tpFreq = args->args[ARG_VALUE_1];
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
break;
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_NO_ERR, 0, NULL);
default:
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_OS_UNKNOWN_COMMAND,
0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
break;
//>>>> oobdtemple protocol MSG_OTHERS >>>>
case FBID_BUS_GENERIC:
case FBID_BUS_SPEC:
actBus_param(args); /* forward the received params to the underlying bus. */
break;
default:
createCommandResultMsg(FBID_PROTOCOL_SPEC,
ERR_CODE_OS_UNKNOWN_COMMAND,
0,
ERR_CODE_OS_UNKNOWN_COMMAND_TEXT);
break;
}
//<<<< oobdtemple protocol MSG_OTHERS <<<<
//>>>> oobdtemple protocol MSG_INIT >>>>
case MSG_INIT:
DEBUGPRINT("Reset Protocol\n", 'a');
if (protocolBuffer != NULL) {
protocolBuffer->len = 0;
}
//<<<< oobdtemple protocol MSG_INIT <<<<
//>>>> oobdtemple protocol MSG_PROTOCOL_STOP >>>>
break;
case MSG_PROTOCOL_STOP:
DEBUGPRINT("Stop Protocol\n", 'a');
keeprunning = 0;
break;
//<<<< oobdtemple protocol MSG_PROTOCOL_STOP <<<<
//>>>> oobdtemple protocol MSG_SEND_BUFFER >>>>
case MSG_SEND_BUFFER:
/* let's Dance: Starting the transfer protocol */
//<<<< oobdtemple protocol MSG_SEND_BUFFER <<<<
if (protocolBuffer->len > 0) {
actDataPacket.recv = protocolConfig->recvID;
actDataPacket.data = &telegram;
actDataPacket.len = 8;
if (protocolBuffer->len < 8) { /* its just single frame */
odp_uds_data2CAN(&protocolBuffer->data[0],
&telegram, protocolBuffer->len,
1);
actDataPacket.data[0] = protocolBuffer->len;
protocolBuffer->len = 0; /* prepare buffer to receive */
actBufferPos = 0;
if (showBusTransfer > 0) {
odp_uds_dumpFrame(&actDataPacket,
printdata_CAN);
}
actBus_send(&actDataPacket);
stateMachine_state = SM_UDS_WAIT_FOR_ANSWER;
timeout = protocolConfig->timeout;
} else { /* we have to send multiframes */
odp_uds_data2CAN(&protocolBuffer->data[0],
&telegram, 6, 2);
actDataPacket.data[0] = 0x10 + (protocolBuffer->len / 256); /* prepare FF */
actDataPacket.data[1] = protocolBuffer->len % 256;
sequenceCounter = 0;
remainingBytes = protocolBuffer->len - 6;
actBufferPos = 6;
protocolBuffer->len = 0; /* prepare buffer to receive */
if (showBusTransfer > 0) {
odp_uds_dumpFrame(&actDataPacket,
printdata_CAN);
}
actBus_send(&actDataPacket);
stateMachine_state = SM_UDS_WAIT_FOR_FC;
timeout = protocolConfig->timeout;
}
//>>>> oobdtemple protocol MSG_SEND_BUFFER_2 >>>>
} else { /* no data to send? */
createCommandResultMsg
(FBID_PROTOCOL_GENERIC, ERR_CODE_NO_ERR, 0, NULL);
DEBUGPRINT("Send input task release msg\n", 'a');
/* just release the input again */
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE, inputQueue, NULL)) {
DEBUGPRINT
("FATAL ERROR: input queue is full!\n", 'a');
}
}
break;
//<<<< oobdtemple protocol MSG_SEND_BUFFER_2 <<<<
//>>>> oobdtemple protocol MSG_TICK >>>>
case MSG_TICK:
//<<<< oobdtemple protocol MSG_TICK <<<<
if (timeout > 0) { /* we just waiting for an answer */
if (timeout == 1) { /* time's gone... */
protocolBuffer->len = 0;
DEBUGPRINT("Timeout!\n", 'a');
createCommandResultMsg(FBID_PROTOCOL_GENERIC,
ERR_CODE_UDS_TIMEOUT, 0,
ERR_CODE_UDS_TIMEOUT_TEXT);
stateMachine_state = SM_UDS_STANDBY;
if (pdPASS !=
sendMsg(MSG_SERIAL_RELEASE, inputQueue,
NULL)) {
DEBUGPRINT
("FATAL ERROR: input queue is full!\n",
'a');
}
}
timeout--;
}
if (actSeparationTime_STTicks > 0) {
DEBUGPRINT
("Remaining CF Waitticks: %ld , remainingBytes: %ld\n",
actSeparationTime_STTicks, remainingBytes);
stateMachine_state = SM_UDS_SLEEP_UNTIL_SINGLE_CF;
actSeparationTime_STTicks--;
if (actSeparationTime_STTicks < 1) { //it's time for a new single CF
stateMachine_state = SM_UDS_SEND_SINGLE_CF;
actSeparationTime_STTicks = separationTime_ST / portTICK_PERIOD_MS; //"reload" the counter
actSeparationTime_STTicks++;
if (actSeparationTime_STTicks < 2) {
actSeparationTime_STTicks = 2;
}
DEBUGPRINT
("Reloaded CF Waitticks: %ld , remainingBytes: %ld\n",
actSeparationTime_STTicks, remainingBytes);
}
}
/* Start generating tester present messages */
odp_uds_generateTesterPresents(tpList,
&telegram, actBus_send);
//>>>> oobdtemple protocol final >>>>
break;
}
//if (Ticker oder sonstiges Consecutife Frame){
if (1) {
if (stateMachine_state == SM_UDS_SEND_CF
|| stateMachine_state == SM_UDS_SEND_SINGLE_CF) {
while (remainingBytes > 0
&& (stateMachine_state !=
SM_UDS_SLEEP_UNTIL_SINGLE_CF)
&& (actBlockSize_BS != 1)) {
if (stateMachine_state == SM_UDS_SEND_SINGLE_CF) {
stateMachine_state =
SM_UDS_SLEEP_UNTIL_SINGLE_CF;
}
DEBUGPRINT("Remaining bytes: %ld\n",
remainingBytes);
actFrameLen =
remainingBytes > 7 ? 7 : remainingBytes;
odp_uds_data2CAN(&protocolBuffer->data
[actBufferPos],
&telegram[0], actFrameLen, 1);
sequenceCounter =
sequenceCounter < 15 ? sequenceCounter + 1 : 0;
actBufferPos += actFrameLen;
remainingBytes -= actFrameLen;
actDataPacket.data[0] = 0x20 + sequenceCounter; // prepare CF
if (showBusTransfer > 0) {
odp_uds_dumpFrame(&actDataPacket,
printdata_CAN);
}
actBus_send(&actDataPacket);
if (actBlockSize_BS > 1) {
actBlockSize_BS--;
DEBUGPRINT("Blocksize REDUCED to %ld \n",
actBlockSize_BS);
}
}
if (actBlockSize_BS == 1) { //in case we had some block limitations, send them and then wait for another FC Frame
stateMachine_state = SM_UDS_WAIT_FOR_FC;
actBlockSize_BS = 0;
timeout = protocolConfig->timeout;
}
if (remainingBytes < 1) { // Buffer empty? Then finish
stateMachine_state = SM_UDS_WAIT_FOR_ANSWER;
actSeparationTime_STTicks = 0;
timeout = protocolConfig->timeout;
}
}
}
disposeMsg(msg);
}
/* vTaskDelay (5000 / portTICK_PERIOD_MS); */
}
/* Do all cleanup here to finish task */
actBus_close();
vPortFree(protocolConfig);
freeODPBuffer(protocolBuffer);
odp_uds_freeTPBuffers(tpList);
xSemaphoreGive(protocollBinarySemaphore);
vTaskDelete(NULL);
}
int main(int argc, char *argv[])
{
fd_set rset;
struct sockaddr_in si_remote, hole_si;
int slen = sizeof(struct sockaddr_in);
int ret, i;
MSG_T msg;
int send_sock = initSendSock();
memset(&si_remote, 0, sizeof(struct sockaddr_in));
fillRemoteInfo(SERVER_IP, SERVER_PORT, &si_remote);
memset(&msg, 0, sizeof(MSG_T));
printf("Input a UUID for registering: ");
scanf("%d", &g_UUID);
////////////////////////////////////////
if (0 != register_UUID(send_sock, &si_remote))
{
printf("Register %d failed\n", g_UUID);
return -1;
}
/////////////////////////////////////////////
printf("1. Send NAT msg.\n2. Recv NAT msg.\n");
printf("# ");
scanf("%d", &ret);
if (1 == ret) // send 2
{
int uuid;
printf("Input a UUID for sending: ");
scanf("%d", &uuid);
memset(&msg, 0, sizeof(MSG_T));
msg.event = APPLY_MAKE_A_HOLE;
msg.SRC_UUID = g_UUID;
msg.DEST_UUID = uuid;
if (0 != sendRequest(send_sock, &si_remote, &msg))
{
printf("[%d]UUID %d want make_a_hole to UUID %d. Return -> failed\n", __LINE__, g_UUID, uuid);
return -1;
}
printf("[%d]UUID %d want make_a_hole to UUID %d. Return -> succeed \n", __LINE__, g_UUID, uuid);
waitMsg(send_sock, &si_remote, &msg);
// all thing done, than start sending
if (HOLE_IS_READY == getEvent(&msg))
{
for (i=0; i<3; i++)
{
if (-1 == sendto(send_sock, "Hello, I'm here", sizeof("Hello, I'm here"), 0, (struct sockaddr*)&msg.nat_si, slen))
{
perror("sendto()");
return -1;
}
}
printf("[%d]Local UUID %d send Nat[%s:%d] Msg to UUID %d finish.\n", __LINE__, getDESTUUID(&msg), inet_ntoa(msg.nat_si.sin_addr), ntohs(msg.nat_si.sin_port), getSRCUUID(&msg));
}
else
{
printf("[%d]The event should be HOLE_IS_READY, but %s is not what I want.\n", __LINE__, getEventStr(&msg));
}
}
else //recv 1
{
MSG_T req_msg;
int port = 65330, recv_sock;
char buff[20] = {0};
struct sockaddr_in si_recv;
//printf("Input a local recv port: ");
//scanf("%d", &port);
recv_sock = initSendSock();
printf("[%d]Start waiting MAKE_A_HOLE ...\n", __LINE__);
memset(&msg, 0, sizeof(MSG_T));
memset(&si_remote, 0, sizeof(struct sockaddr_in));
if (recvfrom(send_sock, &msg, sizeof(MSG_T), 0, (struct sockaddr*)&si_remote, &slen) == -1)
{
perror("recvfrom");
return -1;
}
if (MAKE_A_HOLE == getEvent(&msg))
{
printf("[%d]From UUID %d want make_a_hole to UUID %d, Return -> ",__LINE__, getSRCUUID(&msg), getDESTUUID(&msg));
// punching a hole
memset(&req_msg, 0, sizeof(MSG_T));
memcpy(&hole_si, &msg.nat_si, sizeof(struct sockaddr_in));
if (0 != sendOneWay(recv_sock, &msg.nat_si, &req_msg))
{
printf("punching failed. ");
}
else
{
printf("punching succeed. ");
}
memset(&req_msg, 0, sizeof(MSG_T));
req_msg.event = HOLE_IS_READY;
req_msg.SRC_UUID = getDESTUUID(&msg);
req_msg.DEST_UUID = getSRCUUID(&msg);
// reply hole is ready
if (0 != sendOneWay(send_sock, &si_remote, &req_msg))
{
printf("reply server failed.\n");
}
else
{
printf("reply server succeed.\n");
}
//printf("[%s][%d][%s]\n", __FILE__, __LINE__, __FUNCTION__);
printf("[%d]Reply MAKE_A_HOLE succeed, and wait msg from the hole ...%s:%d\n", __LINE__, inet_ntoa(hole_si.sin_addr), ntohs(hole_si.sin_port));
// all thing ready, than start retrieving
if (recvfrom(recv_sock, &buff[0], sizeof(buff), 0, (struct sockaddr*)&hole_si, &slen) == -1)
{
return -1;
}
printf("Recv from NAT: %s\n", buff);
}
else
{
printf("[%d]The event should be MAKE_A_HOLE, but %s is not what I want.\n", __LINE__, getEventStr(&msg));
}
}
}