/*
* FUNCTION mtc_send_hdlc_packet.
*/
static void mtc_send_hdlc_packet(byte * pBuf, int len)
{
int i;
struct mtc_data_buffer *mb;
word crc = CRC_16_L_SEED;
mb = kzalloc(sizeof(struct mtc_data_buffer), GFP_ATOMIC);
if (mb == NULL) {
printk(KERN_ERR "[MTC] %s: failed to alloc memory\n", __func__);
return;
}
//Generate crc data.
for (i = 0; i < len; i++) {
add_hdlc_esc_packet(mb, pBuf[i]);
crc = CRC_16_L_STEP(crc, (word) pBuf[i]);
}
crc ^= CRC_16_L_SEED;
add_hdlc_esc_packet(mb, ((unsigned char)crc));
add_hdlc_esc_packet(mb, ((unsigned char)((crc >> 8) & 0xFF)));
add_hdlc_packet(mb, CONTROL_CHAR);
if (diagchar_ioctl(DIAG_IOCTL_BULK_DATA, (unsigned long)mb)) {
printk(KERN_ERR "[MTC] %s: ioctl ignored\n", __func__);
}
kfree(mb);
mb = NULL;
}
static uint16
fs_compute_crc (void *data, int byte_count)
{
uint16 crc;
uint8 *bytes = (uint8 *)data;
crc = CRC_16_L_SEED;
while (byte_count--)
crc = CRC_16_L_STEP (crc, bytes[byte_count]);
return crc;
}
/* LGE_CHANGE_E [[email protected]] 2010-02-22, LG_FW_MTC */ void diag_hdlc_encode(struct diag_send_desc_type *src_desc, struct diag_hdlc_dest_type *enc) { uint8_t *dest; uint8_t *dest_last; const uint8_t *src; const uint8_t *src_last; uint16_t crc; unsigned char src_byte = 0; enum diag_send_state_enum_type state; unsigned int used = 0; if (src_desc && enc) { /* Copy parts to local variables. */ src = src_desc->pkt; src_last = src_desc->last; state = src_desc->state; dest = enc->dest; dest_last = enc->dest_last; if (state == DIAG_STATE_START) { crc = CRC_16_L_SEED; state++; } else { /* Get a local copy of the CRC */ crc = enc->crc; } /* dest or dest_last may be NULL to trigger a state transition only */ if (dest && dest_last) { /* This condition needs to include the possibility of 2 dest bytes for an escaped byte */ while (src <= src_last && dest <= dest_last) { src_byte = *src++; if ((src_byte == CONTROL_CHAR) || (src_byte == ESC_CHAR)) { /* If the escape character is not the last byte */ if (dest != dest_last) { crc = CRC_16_L_STEP(crc, src_byte); *dest++ = ESC_CHAR; used++; *dest++ = src_byte ^ ESC_MASK; used++; } else { src--; break; } } else { crc = CRC_16_L_STEP(crc, src_byte); *dest++ = src_byte; used++; } } if (src > src_last) { if (state == DIAG_STATE_BUSY) { if (src_desc->terminate) { crc = ~crc; state++; } else { /* Done with fragment */ state = DIAG_STATE_COMPLETE; } } while (dest <= dest_last && state >= DIAG_STATE_CRC1 && state < DIAG_STATE_TERM) { /* Encode a byte of the CRC next */ src_byte = crc & 0xFF; if ((src_byte == CONTROL_CHAR) || (src_byte == ESC_CHAR)) { if (dest != dest_last) { *dest++ = ESC_CHAR; used++; *dest++ = src_byte ^ ESC_MASK; used++; crc >>= 8; } else { break; } } else { crc >>= 8; *dest++ = src_byte; used++; } state++; }
void diag_hdlc_encode(struct diag_send_desc_type *src_desc,
struct diag_hdlc_dest_type *enc)
{
uint8_t *dest;
uint8_t *dest_last;
const uint8_t *src;
const uint8_t *src_last;
uint16_t crc;
unsigned char src_byte = 0;
enum diag_send_state_enum_type state;
unsigned int used = 0;
if (src_desc && enc) {
src = src_desc->pkt;
src_last = src_desc->last;
state = src_desc->state;
dest = enc->dest;
dest_last = enc->dest_last;
if (state == DIAG_STATE_START) {
crc = CRC_16_L_SEED;
state++;
} else {
crc = enc->crc;
}
if (dest && dest_last) {
while (src <= src_last && dest <= dest_last) {
src_byte = *src++;
if ((src_byte == CONTROL_CHAR) ||
(src_byte == ESC_CHAR)) {
if (dest != dest_last) {
crc = CRC_16_L_STEP(crc,
src_byte);
*dest++ = ESC_CHAR;
used++;
*dest++ = src_byte
^ ESC_MASK;
used++;
} else {
src--;
break;
}
} else {
crc = CRC_16_L_STEP(crc, src_byte);
*dest++ = src_byte;
used++;
}
}
if (src > src_last) {
if (state == DIAG_STATE_BUSY) {
if (src_desc->terminate) {
crc = ~crc;
state++;
} else {
state = DIAG_STATE_COMPLETE;
}
}
while (dest <= dest_last &&
state >= DIAG_STATE_CRC1 &&
state < DIAG_STATE_TERM) {
src_byte = crc & 0xFF;
if ((src_byte == CONTROL_CHAR)
|| (src_byte == ESC_CHAR)) {
if (dest != dest_last) {
*dest++ = ESC_CHAR;
used++;
*dest++ = src_byte ^
ESC_MASK;
used++;
crc >>= 8;
} else {
break;
}
} else {
crc >>= 8;
*dest++ = src_byte;
used++;
}
state++;
}
