/*
* Write data in pg-flash
*
*/
void write_pg_flash_msdp(__msdp_out_data * const pOutData, const __msdp_wr_data * const pWrData) {
pOutData->len = 0;
if (pWrData->len > 0 && pWrData->len < 27) {
// header
pOutData->pBuff[pOutData->len++] = SDP_HEADER_x07;
pOutData->pBuff[pOutData->len++] = SDP_HEADER_x0E;
// data len
pOutData->pBuff[pOutData->len++] = pWrData->len + 4; // cmd(1byte) + addr(3bytes) + data
// cmd 'W'
pOutData->pBuff[pOutData->len++] = 0x35;
pOutData->pBuff[pOutData->len++] = 0x37;
// addr
__union_addr uaddr = {pWrData->addr};
for (uint16_t i = 0; i < 3; i++) {
pOutData->pBuff[pOutData->len++] = nibble_to_hex(uaddr.data8[2 - i] >> 4, 'X');
pOutData->pBuff[pOutData->len++] = nibble_to_hex(uaddr.data8[2 - i] & 0xF, 'X');
}
// data
for (uint16_t i = 0; i < pWrData->len; i++) {
pOutData->pBuff[pOutData->len++] = nibble_to_hex(pWrData->pBuff[i] >> 4, 'X');
pOutData->pBuff[pOutData->len++] = nibble_to_hex(pWrData->pBuff[i] & 0xF, 'X');
}
// crc
pOutData->pBuff[pOutData->len] = calc_crc_sdp(pOutData->pBuff + 3, pOutData->len - 3);
pOutData->len++;
}
static void to_hex(uint8_t* input, size_t len, char* output) {
for (int i = 0; i < len; ++i) {
uint8_t ch = input[i];
uint8_t hi = (ch & 0xF0) >> 4;
uint8_t lo = (ch & 0x0F);
output[2*i] = nibble_to_hex(hi);
output[2*i + 1] = nibble_to_hex(lo);
}
output[2*len] = '\0';
}
