RP-01/cva_bootloader_m0146/SDK/middleware/uds/user/uds_service22.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);
    }

}