127 lines
4.3 KiB
C
127 lines
4.3 KiB
C
/*
|
|
* Copyright (c) 2022, Shenzhen CVA Innovation CO.,LTD
|
|
* All rights reserved.
|
|
*
|
|
* Shenzhen CVA Innovation CO.,LTD (CVA chip) is supplying this file for use
|
|
* exclusively with CVA's microcontroller products. This file can be freely
|
|
* distributed within development tools that are supporting such microcontroller
|
|
* products.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
|
|
* OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
|
|
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
|
|
* CVA SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL,
|
|
* OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
|
|
*/
|
|
|
|
/*******************************************************************************
|
|
* the includes
|
|
******************************************************************************/
|
|
#include <string.h>
|
|
#include "uds_service22.h"
|
|
|
|
|
|
/*******************************************************************************
|
|
* the defines
|
|
******************************************************************************/
|
|
|
|
/*******************************************************************************
|
|
* the typedefs
|
|
******************************************************************************/
|
|
|
|
/*******************************************************************************
|
|
* the globals
|
|
******************************************************************************/
|
|
|
|
/*******************************************************************************
|
|
* the constants
|
|
******************************************************************************/
|
|
|
|
/*******************************************************************************
|
|
* the functions
|
|
******************************************************************************/
|
|
|
|
void UdsService22_ReadDataByIdentifier(UdsType *obj, const uint8_t msgBuf[], uint16_t msgLen)
|
|
{
|
|
uint16_t didNum = 0;
|
|
uint16_t didValue = 0;
|
|
uint16_t rspLen = 0;
|
|
uint16_t msgPos = 0, didIdx = 0;
|
|
bool findDid = false;
|
|
uint8_t rspBuffer[UDS_RSP_LEN_MAX] = {0};
|
|
|
|
if(msgLen < obj->seviceTable[obj->curServiceIdx].minLen)
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_INVALID_MESSAGE_LENGTH_OR_FORMAT);
|
|
return;
|
|
}
|
|
|
|
if(0 != (msgLen - 1) % sizeof(uint16_t))
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_INVALID_MESSAGE_LENGTH_OR_FORMAT);
|
|
return;
|
|
}
|
|
|
|
didNum = msgLen / sizeof(uint16_t);
|
|
if(0 == didNum)
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_INVALID_MESSAGE_LENGTH_OR_FORMAT);
|
|
return;
|
|
}
|
|
|
|
rspBuffer[rspLen++] = UDS_GET_POSITIVE_RSP(0x22);
|
|
|
|
for(msgPos = 1; msgPos < msgLen; msgPos += 2)
|
|
{
|
|
didValue = ((uint16_t)msgBuf[msgPos]) << 8;
|
|
didValue |= msgBuf[msgPos + 1];
|
|
|
|
findDid = false;
|
|
for(didIdx = 0; didIdx < obj->didNum; didIdx++)
|
|
{
|
|
if(obj->didTable[didIdx].did == didValue)
|
|
{
|
|
if(obj->securityLevel != obj->didTable[didIdx].securityLevel)
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_SECURITY_ACCESS_DENIED);
|
|
return;
|
|
}
|
|
else if((rspLen + 2 + obj->didTable[didIdx].length) > UDS_RSP_LEN_MAX)
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_RESPONSE_TOO_LONG);
|
|
return;
|
|
}
|
|
|
|
if((UDS_DID_TYPE_NVM_WO == obj->didTable[didIdx].type) || (obj->session < obj->didTable[didIdx].sessionLevel))
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_CONDITIONS_NOT_CORRECT);
|
|
return;
|
|
}
|
|
obj->didTable[didIdx].function(obj, obj->didTable[didIdx].pBytes, obj->didTable[didIdx].length);
|
|
findDid = true;
|
|
rspBuffer[rspLen++] = msgBuf[msgPos];
|
|
rspBuffer[rspLen++] = msgBuf[msgPos + 1];
|
|
memcpy(&rspBuffer[rspLen], obj->didTable[didIdx].pBytes, obj->didTable[didIdx].length);
|
|
rspLen += obj->didTable[didIdx].length;
|
|
break;
|
|
}
|
|
}
|
|
if(false == findDid)
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
if(true == findDid)
|
|
{
|
|
Uds_PositiveResponse(obj, rspBuffer, rspLen);
|
|
}
|
|
else
|
|
{
|
|
Uds_NegativeResponse(obj, 0x22, NRC_REQUEST_OUT_OF_RANGE);
|
|
}
|
|
|
|
}
|
|
|
|
|