static
lsp_uint8
_lsp_send_pdu(
lsp_handle_context *context,
lsp_pdu_pointers *pdu,
lsp_uint8 *send_buffer,
lsp_uint32 send_buffer_size
)
{
lsp_error_t err;
lsp_trans_error_t err_trans;
lsp_uint32 dataseg_len;
lsp_uint16 ahs_len;
lsp_session_data* session = &context->session;
lsp_transport_proc* trans = &context->proc;
size_t sent, sizesend;
void *wait_handle_pdu = 0, *wait_handle_data = 0;
if(!context)
return 1;
ahs_len = lsp_ntohs(pdu->header_ptr->ahs_len);
dataseg_len = lsp_ntohl(pdu->header_ptr->dataseg_len);
if(LSP_PROTO_VERSION_1_0 == session->HWProtoVersion &&
(0 != ahs_len /* || dataseg_len < 0 */ ))
{
return 2;
}
else if(LSP_PROTO_VERSION_1_1 != session->HWProtoVersion &&
(ahs_len < 0 /* || dataseg_len < 0 */))
{
return 3;
}
// encryption
if(LSP_FULL_FEATURE_PHASE == session->iSessionPhase)
{
// encrypt header
if(session->iHeaderEncryptAlgo)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->header_ptr,
sizeof(lsp_pdu_hdr),
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
if(LSP_PROTO_VERSION_1_0 != session->HWProtoVersion &&
ahs_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->ahs_ptr,
ahs_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// encrypt data
if(session->iDataEncryptAlgo && dataseg_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->data_seg_ptr,
dataseg_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// send request
// typedef lsp_error_t (lsp_proc_call *lstproc_send)(void* context, const void* data, size_t len);
sizesend= sizeof(lsp_pdu_hdr) + ahs_len + dataseg_len;
err_trans = trans->send(context->proc_context, pdu->header_ptr, sizesend, &sent, &wait_handle_pdu);
if(LSP_TRANS_SUCCESS != err_trans || (!wait_handle_pdu && sizesend != sent))
return 4;
if(send_buffer)
{
// encrypt data
if(session->iDataEncryptAlgo)
{
lsp_encrypt32(
(lsp_uint8 *)send_buffer,
send_buffer_size,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
err_trans = trans->send(context->proc_context, (lsp_uint8 *)send_buffer,
send_buffer_size, &sent, &wait_handle_data);
if(LSP_TRANS_SUCCESS != err_trans || (!wait_handle_pdu && send_buffer_size != sent))
return 5;
}
if(wait_handle_pdu)
{
err_trans = trans->wait(context->proc_context, &sent, wait_handle_pdu);
if(LSP_TRANS_SUCCESS != err_trans)
return 6;
wait_handle_pdu = 0;
if(sizesend != sent)
return 7;
}
if(wait_handle_data)
{
err_trans = trans->wait(context->proc_context, &sent, wait_handle_data);
if(LSP_TRANS_SUCCESS != err_trans)
return 8;
wait_handle_data = 0;
if(send_buffer_size != sent)
return 9;
}
return 0;
}
static
lsp_uint8
_lsp_send_pdu(
lsp_handle_context *context,
lsp_pdu_pointers *pdu,
lsp_uint8 *send_buffer,
lsp_uint32 send_buffer_size)
{
lsp_error_t err;
lsp_trans_error_t err_trans;
lsp_uint32 dataseg_len;
lsp_uint16 ahs_len;
lsp_session_data* session = &context->session;
lsp_transport_proc* trans = &context->proc;
size_t sent, sizesend;
void *wait_handle_pdu = 0, *wait_handle_data = 0;
if (!context)
{
return LSP_SPE_INVALID_CONTEXT;
}
ahs_len = lsp_ntohs(pdu->header_ptr->ahs_len);
dataseg_len = lsp_ntohl(pdu->header_ptr->dataseg_len);
if (LSP_PROTO_VERSION_1_0 == session->HWProtoVersion &&
(0 != ahs_len))
{
/* 1.0: ahs_len should be 0 */
return LSP_SPE_INVALID_AHS_LEN;
}
else if (LSP_PROTO_VERSION_1_1 == session->HWProtoVersion &&
(0 != dataseg_len))
{
/* 1.1: data_seg should be 0 */
return LSP_SPE_INVALID_DSG_LEN; /* 3; */
}
// encryption
if (LSP_FULL_FEATURE_PHASE == session->iSessionPhase)
{
// encrypt header
if (session->iHeaderEncryptAlgo)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->header_ptr,
sizeof(lsp_pdu_hdr),
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
if (LSP_PROTO_VERSION_1_0 != session->HWProtoVersion &&
ahs_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->ahs_ptr,
ahs_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// encrypt data
if (session->iDataEncryptAlgo && dataseg_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->data_seg_ptr,
dataseg_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// send request
// send pdu header, ahs, dataseg at once
sizesend= sizeof(lsp_pdu_hdr) + ahs_len + dataseg_len;
err_trans = trans->send(
context->proc_context,
pdu->header_ptr,
sizesend,
&sent,
&wait_handle_pdu);
if (LSP_TRANS_SUCCESS != err_trans || (!wait_handle_pdu && sizesend != sent))
{
return LSP_SPE_PDU_SEND_FAIL;
}
if (send_buffer)
{
// encrypt data
if (session->iDataEncryptAlgo)
{
lsp_encrypt32(
(lsp_uint8 *)send_buffer,
send_buffer_size,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
err_trans = trans->send(
context->proc_context,
(lsp_uint8 *)send_buffer,
send_buffer_size,
&sent,
&wait_handle_data);
if (LSP_TRANS_SUCCESS != err_trans || (!wait_handle_pdu && send_buffer_size != sent))
{
return LSP_SPE_DAT_SEND_FAIL;
}
}
if (wait_handle_pdu)
{
err_trans = trans->wait(context->proc_context, &sent, wait_handle_pdu);
if (LSP_TRANS_SUCCESS != err_trans)
{
return LSP_SPE_PDU_SEND_WAIT_FAIL;
}
wait_handle_pdu = 0;
if (sizesend != sent)
{
return LSP_SPE_PDU_SEND_INVALID_LEN;
}
}
if (wait_handle_data)
{
err_trans = trans->wait(context->proc_context, &sent, wait_handle_data);
if (LSP_TRANS_SUCCESS != err_trans)
{
return LSP_SPE_DAT_SEND_WAIT_FAIL;
}
wait_handle_data = 0;
if (send_buffer_size != sent)
{
return LSP_SPE_DAT_SEND_INVALID_LEN;
}
}
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_send_pdu(
lsp_handle_context *context,
lsp_pdu_pointers *pdu)
{
lsp_error_t err;
lsp_trans_error_t err_trans;
lsp_uint32 dataseg_len;
lsp_uint16 ahs_len;
lsp_session_data* session = &context->session;
lsp_transport_proc* trans = &context->proc;
size_t sent, sizesend;
if(!context)
return LSP_ERR_INVALID_HANDLE;
ahs_len = lsp_ntohs(pdu->header_ptr->ahs_len);
dataseg_len = lsp_ntohl(pdu->header_ptr->dataseg_len);
if(LSP_PROTO_VERSION_1_0 == session->HWProtoVersion &&
(0 != ahs_len || dataseg_len < 0))
{
return LSP_ERR_INVALID_PARAMETER;
}
else if(LSP_PROTO_VERSION_1_1 != session->HWProtoVersion &&
(ahs_len < 0 || dataseg_len < 0))
{
return LSP_ERR_INVALID_PARAMETER;
}
// encryption
if(LSP_FULL_FEATURE_PHASE == session->iSessionPhase)
{
// encrypt header
if(session->iHeaderEncryptAlgo)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->header_ptr,
sizeof(lsp_pdu_hdr),
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
if(LSP_PROTO_VERSION_1_0 != session->HWProtoVersion &&
ahs_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->ahs_ptr,
ahs_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// encrypt data
if(session->iDataEncryptAlgo && dataseg_len > 0)
{
lsp_encrypt32(
(lsp_uint8 *)pdu->data_seg_ptr,
dataseg_len,
(lsp_uint8 *)&session->CHAP_C,
(lsp_uint8 *)&session->iPassword);
}
}
// send request
// typedef lsp_error_t (lsp_proc_call *lstproc_send)(void* context, const void* data, size_t len);
sizesend= sizeof(lsp_pdu_hdr) + ahs_len + dataseg_len;
err_trans = trans->send(context->proc_context, pdu->header_ptr, sizesend, &sent);
if(LSP_TRANS_SUCCESS != err_trans)
return ERROR_T_COMPOSITE(LSP_ERR_FUNC_SEND_PDU, 0, LSP_ERR_TYPE_SEND_PDU, 0);
if(sizesend != sent)
return LSP_ERR_SEND_FAILED;
return LSP_ERR_SUCCESS;
}
