static lsp_uint8 _lsp_recv_pdu( lsp_handle_context *context, lsp_uint8 *buffer, lsp_pdu_pointers *pdu, lsp_uint8 *recv_buffer, lsp_uint32 recv_size) { lsp_error_t err; lsp_trans_error_t err_trans; lsp_uint16 ahs_len; lsp_uint32 dataseg_len; lsp_session_data* session = &context->session; lsp_transport_proc* trans = &context->proc; size_t recvd; if (!context) { return LSP_RPE_INVALID_CONTEXT; } if (recv_buffer) { err_trans = trans->recv( context->proc_context, (lsp_uint8 *)recv_buffer, recv_size, &recvd, 0); if (LSP_TRANS_SUCCESS != err_trans) { return LSP_RPE_DAT_RECV_FAIL; } if (recv_size != recvd) { return LSP_RPE_DAT_INVALID_LEN; } /* decrypt data */ if (session->iDataEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)recv_buffer, recv_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } /* read lsp_pdu_hdr */ err_trans = trans->recv( context->proc_context, buffer, sizeof(lsp_pdu_hdr), &recvd, 0); if (LSP_TRANS_SUCCESS != err_trans) { return LSP_RPE_HDR_RECV_FAIL; } if (sizeof(lsp_pdu_hdr) != recvd) { return LSP_RPE_HDR_INVALID_LEN; } pdu->header_ptr = (lsp_pdu_hdr *)buffer; buffer += sizeof(lsp_pdu_hdr); if (LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->header_ptr, sizeof(lsp_pdu_hdr), (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } /* read ahs */ ahs_len = lsp_ntohs(pdu->header_ptr->ahs_len); if (ahs_len > 0) { err_trans = trans->recv( context->proc_context, buffer, ahs_len, &recvd, 0); if (LSP_TRANS_SUCCESS != err_trans) { return LSP_RPE_AHS_RECV_FAIL; } if (ahs_len != recvd) { return LSP_RPE_AHS_INVALID_LEN; } pdu->ahs_ptr = (lsp_uint8 *)buffer; buffer += ahs_len; if (LSP_PROTO_VERSION_1_0 != session->HWProtoVersion && LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->ahs_ptr, ahs_len, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } // read header dig pdu->header_dig_ptr = 0; // read data segment dataseg_len = lsp_ntohl(pdu->header_ptr->dataseg_len); if (dataseg_len > 0) { err_trans = trans->recv(context->proc_context, buffer, dataseg_len, &recvd, 0); if (LSP_TRANS_SUCCESS != err_trans) { return LSP_RPE_DSG_RECV_FAIL; } if (dataseg_len != recvd) { return LSP_RPE_DSG_INVALID_LEN; } pdu->data_seg_ptr = (lsp_uint8 *)buffer; buffer += dataseg_len; if (LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->data_seg_ptr, dataseg_len, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } // read data dig pdu->data_dig_ptr = 0; return 0; }
int test( const lsp_uint8_t* refbuf, lsp_uint32_t bufsize, lsp_uint32_t key, lsp_uint8_t* pwd) { const lsp_uint8_t* src; lsp_uint8_t* dst, *bufe, *bufd; lsp_uint32_t ckey; int i; bufe = malloc(bufsize); bufd = malloc(bufsize); if (0 == bufe || 0 == bufd) { free(bufd); free(bufe); fprintf(stderr, "error: out of memory\n"); return -1; } lsp_encrypt32_build_combined_key(&ckey, key, pwd); dst = bufe; src = refbuf; /* lsp_encrypt32 */ memcpy(dst, src, bufsize); lsp_encrypt32(dst, bufsize, key, pwd); fprintf(stdout, "%-20s: %08X\n", "lsp_encrypt32", checksum(dst, bufsize)); /* lsp_encrypt32ex */ memcpy(dst, src, bufsize); lsp_encrypt32ex(dst, bufsize, ckey); fprintf(stdout, "%-20s: %08X\n", "lsp_encrypt32ex", checksum(dst, bufsize)); /* lsp_encrypt32exx */ memcpy(dst, src, bufsize); lsp_encrypt32exx(dst, bufsize, ckey); fprintf(stdout, "%-20s: %08X\n", "lsp_encrypt32exx", checksum(dst, bufsize)); /* decryption test */ dst = bufd; src = bufe; lsp_decrypt32_build_combined_key(&ckey, key, pwd); /* lsp_decrypt32 */ memcpy(dst, src, bufsize); lsp_decrypt32(dst, bufsize, key, pwd); fprintf(stdout, "%-20s: %08X - should_be_zero=%d\n", "lsp_decrypt32", checksum(dst, bufsize), memcmp(dst, refbuf, bufsize)); /* lsp_decrypt32ex */ memcpy(dst, src, bufsize); lsp_decrypt32ex(dst, bufsize, ckey); fprintf(stdout, "%-20s: %08X - should_be_zero=%d\n", "lsp_decrypt32ex", checksum(dst, bufsize), memcmp(dst, refbuf, bufsize)); /* lsp_decrypt32exx */ memcpy(dst, src, bufsize); lsp_decrypt32exx(dst, bufsize, ckey); fprintf(stdout, "%-20s: %08X - should_be_zero=%d\n", "lsp_decrypt32exx", checksum(dst, bufsize), memcmp(dst, refbuf, bufsize)); free(bufd); free(bufe); return 0; }
static lsp_error_t _lsp_ide_command_v1( lsp_handle_context *context, lsp_uint32 target_id, lsp_uint32 lun0, lsp_uint32 lun1, lsp_ide_register_param_ptr p, lsp_io_data_buffer_ptr data_buf) { lsp_error_t err; lsp_trans_error_t err_trans; lsp_uint8 pdu_buffer[LSP_MAX_REQUEST_SIZE]; lsp_pdu_hdr *pdu_hdr; lsp_ide_header_ptr ide_header; lsp_ide_register_ptr ide_register; lsp_pdu_pointers pdu; lsp_session_data* session = &context->session; lsp_transport_proc* trans = &context->proc; lsp_uint32 data_trans_len; size_t sent, recvd; if(LSP_PROTO_VERSION_1_1 != session->HWProtoVersion) return LSP_ERR_INVALID_PARAMETER; session->iCommandTag++; lsp_memset(pdu_buffer, 0x00, LSP_MAX_REQUEST_SIZE); // initialize pdu header pdu_hdr = (lsp_pdu_hdr *)pdu_buffer; pdu_hdr->op_code = LSP_OPCODE_IDE_COMMAND; pdu_hdr->op_flags.ide_command.F = 1; pdu_hdr->op_flags.ide_command.R = (data_buf->recv_buffer && data_buf->recv_size > 0) ? 1 : 0; pdu_hdr->op_flags.ide_command.W = (data_buf->send_buffer && data_buf->send_size > 0) ? 1 : 0; pdu_hdr->hpid = lsp_htonl(session->HPID); pdu_hdr->rpid = lsp_htons(session->RPID); pdu_hdr->cpslot = 0; pdu_hdr->dataseg_len = 0; pdu_hdr->ahs_len = 0; pdu_hdr->cmd_subpkt_seq = 0; pdu_hdr->path_cmd_tag = lsp_htonl(session->iCommandTag); data_trans_len = (pdu_hdr->op_flags.ide_command.W) ? (data_buf->send_size) : (pdu_hdr->op_flags.ide_command.R) ? (data_buf->recv_size) : 0; pdu_hdr->data_trans_len = lsp_htonl(data_trans_len); pdu_hdr->target_id = lsp_htonl(target_id); pdu_hdr->lun0 = lsp_htonl(lun0); pdu_hdr->lun1 = lsp_htonl(lun1); // set ide header ide_header = &(pdu_hdr->op_data.ide_command.header); lsp_memset(ide_header, 0x00, sizeof(lsp_ide_header)); ide_header->com_type_p = (WIN_PACKETCMD == p->command.command) ? 1 : 0; ide_header->com_type_k = 0; ide_header->com_type_d_p = (p->use_dma) ? 1 : 0; ide_header->com_type_w = pdu_hdr->op_flags.ide_command.W; ide_header->com_type_r = pdu_hdr->op_flags.ide_command.R; ide_header->com_type_e = (p->use_48) ? 1 : 0; ide_header->com_len = data_trans_len & 0x03FFFFFF; // 32 -> 26 bit *(lsp_uint32 *)ide_header = lsp_htonl(*(lsp_uint32 *)ide_header); // set ide register ide_register = &(pdu_hdr->op_data.ide_command.register_data); lsp_memset(ide_register, 0x00, sizeof(lsp_ide_register)); ide_register->device = p->device.device; ide_register->command = p->command.command; if(p->use_48) { ide_register->feature_prev = p->reg.named_48.prev.features; ide_register->feature_cur = p->reg.named_48.cur.features; ide_register->sector_count_prev = p->reg.named_48.prev.sector_count; ide_register->sector_count_cur = p->reg.named_48.cur.sector_count; ide_register->lba_low_prev = p->reg.named_48.prev.lba_low; ide_register->lba_low_cur = p->reg.named_48.cur.lba_low; ide_register->lba_mid_prev = p->reg.named_48.prev.lba_mid; ide_register->lba_mid_cur = p->reg.named_48.cur.lba_mid; ide_register->lba_high_prev = p->reg.named_48.prev.lba_high; ide_register->lba_high_cur = p->reg.named_48.cur.lba_high; } else { // set prev == cur for NDAS chip protection ide_register->feature_prev = p->reg.named.features; ide_register->feature_cur = p->reg.named.features; ide_register->sector_count_prev = p->reg.named.sector_count; ide_register->sector_count_cur = p->reg.named.sector_count; ide_register->lba_low_prev = p->reg.named.lba_low; ide_register->lba_low_cur = p->reg.named.lba_low; ide_register->lba_mid_prev = p->reg.named.lba_mid; ide_register->lba_mid_cur = p->reg.named.lba_mid; ide_register->lba_high_prev = p->reg.named.lba_high; ide_register->lba_high_cur = p->reg.named.lba_high; } lsp_memset(&pdu, 0x00, sizeof(lsp_pdu_pointers)); pdu.header_ptr = pdu_hdr; err = _lsp_send_pdu(context, &pdu); if(LSP_ERR_SUCCESS != err) return err; // send data if needed if(data_buf && data_buf->send_buffer) { // encrypt data if(session->iDataEncryptAlgo) { lsp_encrypt32( (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } err_trans = trans->send(context->proc_context, (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, &sent); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 1, LSP_ERR_TYPE_SEND_PDU, 0); if(data_buf->send_size != sent) return LSP_ERR_SEND_FAILED; } // receive data if needed if(data_buf && data_buf->recv_buffer) { err_trans = trans->recv(context->proc_context, (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 2, LSP_ERR_TYPE_RECV_PDU, 0); if(LSP_ERR_SUCCESS != err) return err; if(data_buf->recv_size != recvd) return LSP_ERR_RECV_FAILED; // decrypt data if(session->iDataEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } err = _lsp_recv_pdu(context, pdu_buffer, &pdu); if(LSP_ERR_SUCCESS != err) return err; pdu_hdr = pdu.header_ptr; if(LSP_OPCODE_IDE_RESPONSE != pdu_hdr->op_code) return LSP_ERR_REPLY_FAIL; if(0 == pdu_hdr->op_flags.ide_command.F) return LSP_ERR_COMMAND_FAILED; if(LSP_ERR_RESPONSE_SUCCESS != pdu_hdr->response) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 4, LSP_ERR_TYPE_RESPONSE, pdu_hdr->response); // store results // ide_header = &pdu.header_ptr->op_data.ide_command.header; ide_register = &pdu_hdr->op_data.ide_command.register_data; p->command.command = ide_register->command; // status p->reg.ret.err.err_na = ide_register->feature_cur; // error p->reg.named_48.prev.features = ide_register->feature_prev; // p->reg.named_48.cur.features = ide_register->feature_cur; // err p->reg.named_48.prev.sector_count = ide_register->sector_count_prev; p->reg.named_48.cur.sector_count = ide_register->sector_count_cur; p->reg.named_48.prev.lba_low = ide_register->lba_low_prev; p->reg.named_48.cur.lba_low = ide_register->lba_low_cur; p->reg.named_48.prev.lba_mid = ide_register->lba_mid_prev; p->reg.named_48.cur.lba_mid = ide_register->lba_mid_cur; p->reg.named_48.prev.lba_high = ide_register->lba_high_prev; p->reg.named_48.cur.lba_high = ide_register->lba_high_cur; return LSP_ERR_SUCCESS; }
static lsp_error_t _lsp_ide_command_v0( lsp_handle_context *context, lsp_uint32 target_id, lsp_uint32 lun0, lsp_uint32 lun1, lsp_ide_register_param_ptr p, lsp_io_data_buffer_ptr data_buf) { lsp_error_t err; lsp_trans_error_t err_trans; lsp_uint8 pdu_buffer[LSP_MAX_REQUEST_SIZE]; lsp_pdu_hdr *pdu_hdr; lsp_ide_data_v0 *ide_data_v0_ptr; lsp_pdu_pointers pdu; lsp_session_data* session = &context->session; lsp_transport_proc* trans = &context->proc; size_t sent, recvd; if(LSP_PROTO_VERSION_1_0 != session->HWProtoVersion) return LSP_ERR_INVALID_PARAMETER; session->iCommandTag++; lsp_memset(pdu_buffer, 0x00, LSP_MAX_REQUEST_SIZE); // initialize pdu header pdu_hdr = (lsp_pdu_hdr *)pdu_buffer; pdu_hdr->op_code = LSP_OPCODE_IDE_COMMAND; pdu_hdr->op_flags.ide_command.F = 1; pdu_hdr->hpid = lsp_htonl(session->HPID); pdu_hdr->rpid = lsp_htons(session->RPID); pdu_hdr->cpslot = 0; pdu_hdr->dataseg_len = 0; pdu_hdr->ahs_len = 0; pdu_hdr->cmd_subpkt_seq = 0; pdu_hdr->path_cmd_tag = lsp_htonl(session->iCommandTag); pdu_hdr->target_id = lsp_htonl(target_id); pdu_hdr->lun0 = lsp_htonl(lun0); pdu_hdr->lun1 = lsp_htonl(lun1); ide_data_v0_ptr = &(pdu_hdr->op_data.ide_command_v0); lsp_memset(ide_data_v0_ptr, 0x00, sizeof(lsp_ide_data_v0)); ide_data_v0_ptr->dev = (0 == target_id) ? 0 : 1; // set pdu flags pdu_hdr->op_flags.ide_command.R = 0; pdu_hdr->op_flags.ide_command.W = 0; if(data_buf) { if(data_buf->recv_buffer) pdu_hdr->op_flags.ide_command.R = 1; if(data_buf->send_buffer) pdu_hdr->op_flags.ide_command.W = 1; } // p->use_dma is ignored, V1.0 supports PIO only // set device ide_data_v0_ptr->device = p->device.device; // translate command switch(p->command.command) { case WIN_READ: // case WIN_READDMA: // case WIN_READDMA_EXT: ide_data_v0_ptr->command = (p->use_48) ? WIN_READDMA_EXT : WIN_READDMA; break; case WIN_WRITE: // case WIN_WRITEDMA: // case WIN_WRITEDMA_EXT: ide_data_v0_ptr->command = (p->use_48) ? WIN_WRITEDMA_EXT : WIN_WRITEDMA; break; case WIN_VERIFY: // case WIN_VERIFY_EXT: ide_data_v0_ptr->command = (p->use_48) ? WIN_VERIFY_EXT : WIN_VERIFY; break; case WIN_IDENTIFY: case WIN_SETFEATURES: ide_data_v0_ptr->command = p->command.command; break; default: // V1.0 does not support all the ide commands return LSP_ERR_NOT_SUPPORTED; } // set location, sector count, feature if(p->use_48) { ide_data_v0_ptr->feature = p->reg.named_48.prev.features; ide_data_v0_ptr->sector_count_prev = p->reg.named_48.prev.sector_count; ide_data_v0_ptr->sector_count_cur = p->reg.named_48.cur.sector_count; ide_data_v0_ptr->lba_low_prev = p->reg.named_48.prev.lba_low; ide_data_v0_ptr->lba_low_cur = p->reg.named_48.cur.lba_low; ide_data_v0_ptr->lba_mid_prev = p->reg.named_48.prev.lba_mid; ide_data_v0_ptr->lba_mid_cur = p->reg.named_48.cur.lba_mid; ide_data_v0_ptr->lba_high_prev = p->reg.named_48.prev.lba_high; ide_data_v0_ptr->lba_high_cur = p->reg.named_48.cur.lba_high; } else { ide_data_v0_ptr->feature = p->reg.named.features; ide_data_v0_ptr->sector_count_cur = p->reg.named.sector_count; ide_data_v0_ptr->lba_low_cur = p->reg.named.lba_low; ide_data_v0_ptr->lba_mid_cur = p->reg.named.lba_mid; ide_data_v0_ptr->lba_high_cur = p->reg.named.lba_high; } lsp_memset(&pdu, 0x00, sizeof(lsp_pdu_pointers)); pdu.header_ptr = pdu_hdr; err = _lsp_send_pdu(context, &pdu); if(LSP_ERR_SUCCESS != err) return err; // send data if needed if(data_buf && data_buf->send_buffer) { // encrypt data if(session->iDataEncryptAlgo) { lsp_encrypt32( (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } err_trans = trans->send(context->proc_context, (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, &sent); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 1, LSP_ERR_TYPE_SEND_PDU, 0); if(data_buf->send_size != sent) return LSP_ERR_SEND_FAILED; } // receive data if needed if(data_buf && data_buf->recv_buffer) { err_trans = trans->recv(context->proc_context, (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 2, LSP_ERR_TYPE_RECV_PDU, 0); if(data_buf->recv_size != recvd) return LSP_ERR_RECV_FAILED; // decrypt data if(session->iDataEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } err = _lsp_recv_pdu(context, pdu_buffer, &pdu); if(LSP_ERR_SUCCESS != err) return err; pdu_hdr = pdu.header_ptr; if(LSP_OPCODE_IDE_RESPONSE != pdu_hdr->op_code) return LSP_ERR_REPLY_FAIL; if(LSP_ERR_RESPONSE_SUCCESS != pdu_hdr->response) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_IDE_COMMAND, 4, LSP_ERR_TYPE_RESPONSE, pdu_hdr->response); return LSP_ERR_SUCCESS; }
lsp_error_t lsp_call lsp_vendor_command( lsp_handle h, lsp_uint16 vendor_id, lsp_uint8 vop_ver, lsp_uint8 vop_code, lsp_uint8 *param, lsp_uint8 param_length, lsp_io_data_buffer_ptr data_buf) { lsp_error_t err; lsp_trans_error_t err_trans; lsp_uint8 pdu_buffer[LSP_MAX_REQUEST_SIZE]; lsp_pdu_hdr *pdu_hdr; lsp_ide_header_ptr ide_header; lsp_ide_register_ptr ide_register; lsp_pdu_pointers pdu; lsp_handle_context *context = (lsp_handle_context*) h; lsp_session_data* session = &context->session; lsp_transport_proc* trans = &context->proc; size_t sent, recvd; if(!context) return LSP_ERR_INVALID_HANDLE; if(!param) return LSP_ERR_INVALID_PARAMETER; if(LSP_HW_VERSION_1_0 == session->HWVersion) { if(4 != param_length) return LSP_ERR_INVALID_PARAMETER; } else if(LSP_HW_VERSION_1_1 == session->HWVersion || LSP_HW_VERSION_2_0 == session->HWVersion) { if(8 != param_length) return LSP_ERR_INVALID_PARAMETER; } else return LSP_ERR_INVALID_PARAMETER; if(data_buf && ((data_buf->recv_buffer && 0 == data_buf->recv_size) || (data_buf->send_buffer && 0 == data_buf->send_size))) return LSP_ERR_INVALID_PARAMETER; session->iCommandTag++; lsp_memset(pdu_buffer, 0x00, LSP_MAX_REQUEST_SIZE); // initialize pdu header pdu_hdr = (lsp_pdu_hdr *)pdu_buffer; pdu_hdr->op_code = LSP_OPCODE_VENDOR_SPECIFIC_COMMAND; pdu_hdr->op_flags.ide_command.F = 1; pdu_hdr->hpid = lsp_htonl(session->HPID); pdu_hdr->rpid = lsp_htons(session->RPID); pdu_hdr->cpslot = 0; pdu_hdr->dataseg_len = 0; pdu_hdr->ahs_len = 0; pdu_hdr->cmd_subpkt_seq = 0; pdu_hdr->path_cmd_tag = lsp_htonl(session->iCommandTag); pdu_hdr->op_data.vendor_command.vendor_id = lsp_htons(vendor_id); pdu_hdr->op_data.vendor_command.vop_ver = vop_ver; pdu_hdr->op_data.vendor_command.vop_code = vop_code; lsp_htonx(pdu_hdr->op_data.vendor_command.vop_parm, param, param_length); lsp_memset(&pdu, 0x00, sizeof(lsp_pdu_pointers)); pdu.header_ptr = pdu_hdr; err = _lsp_send_pdu(context, &pdu); if(LSP_ERR_SUCCESS != err) return err; // send data if needed if(data_buf && data_buf->send_buffer) { // encrypt data if(session->iDataEncryptAlgo) { lsp_encrypt32( (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } err_trans = trans->send(context->proc_context, (lsp_uint8 *)data_buf->send_buffer, data_buf->send_size, &sent); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_VENDOR_COMMAND, 1, LSP_ERR_TYPE_SEND_PDU, 0); if(data_buf->send_size != sent) return LSP_ERR_SEND_FAILED; } // receive data if needed if(data_buf && data_buf->recv_buffer) { err_trans = trans->recv(context->proc_context, (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_VENDOR_COMMAND, 2, LSP_ERR_TYPE_RECV_PDU, 0); if(LSP_ERR_SUCCESS != err) return err; if(data_buf->recv_size != recvd) return LSP_ERR_RECV_FAILED; // decrypt data if(session->iDataEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)data_buf->recv_buffer, data_buf->recv_size, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } err = _lsp_recv_pdu(context, pdu_buffer, &pdu); if(LSP_ERR_SUCCESS != err) return err; pdu_hdr = pdu.header_ptr; if(LSP_OPCODE_VENDOR_SPECIFIC_RESPONSE != pdu_hdr->op_code) return LSP_ERR_REPLY_FAIL; if(0 == pdu_hdr->op_flags.vendor_command.F) return LSP_ERR_COMMAND_FAILED; if(LSP_ERR_RESPONSE_SUCCESS != pdu_hdr->response) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_VENDOR_COMMAND, 4, LSP_ERR_TYPE_RESPONSE, pdu_hdr->response); // store results lsp_ntohx(param, pdu_hdr->op_data.vendor_command.vop_parm, param_length); return LSP_ERR_SUCCESS; }
static lsp_error_t _lsp_recv_pdu( lsp_handle_context *context, lsp_uint8 *buffer, lsp_pdu_pointers *pdu) { lsp_error_t err; lsp_trans_error_t err_trans; lsp_uint16 ahs_len; lsp_uint32 dataseg_len; lsp_session_data* session = &context->session; lsp_transport_proc* trans = &context->proc; size_t recvd; // read lsp_pdu_hdr err_trans = trans->recv(context->proc_context, buffer, sizeof(lsp_pdu_hdr), &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_RECV_PDU, 0, LSP_ERR_TYPE_RECV_PDU, 0); if(sizeof(lsp_pdu_hdr) != recvd) return LSP_ERR_RECV_FAILED; pdu->header_ptr = (lsp_pdu_hdr *)buffer; buffer += sizeof(lsp_pdu_hdr); if(LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->header_ptr, sizeof(lsp_pdu_hdr), (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } // read ahs ahs_len = lsp_ntohs(pdu->header_ptr->ahs_len); if(ahs_len > 0) { err_trans = trans->recv(context->proc_context, buffer, ahs_len, &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_RECV_PDU, 1, LSP_ERR_TYPE_RECV_PDU, 0); if(ahs_len != recvd) return LSP_ERR_RECV_FAILED; pdu->ahs_ptr = (lsp_uint8 *)buffer; buffer += ahs_len; if(LSP_PROTO_VERSION_1_0 != session->HWProtoVersion && LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->ahs_ptr, ahs_len, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } // read header dig pdu->header_dig_ptr = 0; // read data segment dataseg_len = lsp_ntohl(pdu->header_ptr->dataseg_len); if(dataseg_len > 0) { err_trans = trans->recv(context->proc_context, buffer, dataseg_len, &recvd); if(LSP_TRANS_SUCCESS != err_trans) return ERROR_T_COMPOSITE(LSP_ERR_FUNC_RECV_PDU, 3, LSP_ERR_TYPE_RECV_PDU, 0); if(dataseg_len != recvd) return LSP_ERR_RECV_FAILED; pdu->data_seg_ptr = (lsp_uint8 *)buffer; buffer += dataseg_len; if(LSP_FULL_FEATURE_PHASE == session->iSessionPhase && session->iHeaderEncryptAlgo) { lsp_decrypt32( (lsp_uint8 *)pdu->data_seg_ptr, dataseg_len, (lsp_uint8 *)&session->CHAP_C, (lsp_uint8 *)&session->iPassword); } } // read data dig pdu->data_dig_ptr = 0; return LSP_ERR_SUCCESS; }