帮我用C写一段CANopen SDO读取后解析函数

以下是一份简单的CANopen SDO读取后解析函数的C代码，其中假设CAN消息已经被接收并存储在了一个名为"msg"的CAN消息结构体中：

// 定义CANopen SDO协议中的一些常量
#define SDO_REQUEST_READ 0x40
#define SDO_RESPONSE_READ 0x43
#define SDO_RESPONSE_WRITE 0x60

// 定义CAN消息结构体
typedef struct can_message {
    uint32_t id;
    uint8_t data[8];
    uint8_t len;
} can_message_t;

// 定义SDO读取后解析函数
int sdo_read_parse(can_message_t* msg, uint16_t* index, uint8_t* subindex, uint8_t* data, uint8_t* data_len) {
    // 检查消息是否为SDO读取响应
    if ((msg->data[0] & 0xE0) != SDO_RESPONSE_READ) {
        return -1;
    }
    // 检查消息是否为SDO读取响应帧
    if (msg->data[0] & 0x10) {
        return -1;
    }
    // 检查索引是否匹配
    uint16_t msg_index = ((uint16_t)msg->data[2] << 8) | msg->data[1];
    if (msg_index != *index) {
        return -1;
    }
    // 检查子索引是否匹配
    uint8_t msg_subindex = msg->data[3];
    if (msg_subindex != *subindex) {
        return -1;
    }
    // 获取数据长度
    *data_len = msg->len - 4;
    // 复制数据到输出缓冲区
    for (int i = 0; i < *data_len; i++) {
        data[i] = msg->data[i+4];
    }
    // 返回成功
    return 0;
}

这个函数接收一个CAN消息结构体作为输入，并将消息中的索引、子索引和数据解析出来，存储到指针变量中。函数返回0表示解析成功，返回-1表示解析失败。