249 lines
6.6 KiB
C
249 lines
6.6 KiB
C
#include "stmisp.h"
|
|
#include <termios.h>
|
|
#include <sys/time.h>
|
|
// Utility: XOR checksum of bytes
|
|
uint8_t xor_checksum(const uint8_t *buf, size_t len) {
|
|
uint8_t cs = 0;
|
|
for (size_t i = 0; i < len; ++i) cs ^= buf[i];
|
|
return cs;
|
|
}
|
|
|
|
int stmisp_init(stmisp_device_t *stmisp_device, int (*send)(void* data, uint16_t len, int timeout), int (*recv)(void* data, uint16_t len, int timeout), int (*get_length )(void))
|
|
{
|
|
stmisp_device->send = send;
|
|
stmisp_device->recv = recv;
|
|
stmisp_device->get_length = get_length;
|
|
return 0;
|
|
}
|
|
|
|
int wait_ack(stmisp_device_t *stmisp_device, uint32_t timeout)
|
|
{
|
|
uint8_t ack = 0;
|
|
uint8_t size =stmisp_device->recv(&ack, 1, timeout);
|
|
|
|
if(size > 0)
|
|
{
|
|
switch (ack)
|
|
{
|
|
case ACK:
|
|
return 1;
|
|
case NACK:
|
|
return 2;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// int wait_ack(stmisp_device_t *stmisp_device, uint32_t timeout) {
|
|
// double elapsed = 0.0;
|
|
// const double step = 0.02; // poll step
|
|
// struct timeval t0;
|
|
// gettimeofday(&t0, NULL);
|
|
// while (elapsed < timeout) {
|
|
// uint8_t b;
|
|
// int r = stmisp_device->recv(&b, 1, 0);
|
|
// if (r < 0) return -1;
|
|
// if (r == 0) {
|
|
// // nothing
|
|
// // printf("stmisp: No ACK (timeout)\n");
|
|
// } else {
|
|
// if (b == ACK) return 1;
|
|
// if (b == NACK) return 2;
|
|
// // else ignore and continue
|
|
// }
|
|
// struct timeval t1;
|
|
// gettimeofday(&t1, NULL);
|
|
// elapsed = (t1.tv_sec - t0.tv_sec) + (t1.tv_usec - t0.tv_usec) / 1e6;
|
|
// }
|
|
// return 0; // timeout
|
|
// }
|
|
|
|
int send_cmd_wait(stmisp_device_t *stmisp_device, uint8_t cmd, uint32_t timeout)
|
|
{
|
|
uint8_t pkt[2];
|
|
pkt[0] = cmd; pkt[1] = cmd ^ 0xFF;
|
|
stmisp_device->send(pkt, 2, 0);
|
|
uint8_t ack = wait_ack(stmisp_device, timeout);
|
|
|
|
switch(ack)
|
|
{
|
|
case 1:
|
|
printf("stmisp: ACK to cmd 0x%02x\n", cmd);
|
|
return 1;
|
|
case 2:
|
|
printf("stmisp: NACK to cmd 0x%02x\n", cmd);
|
|
return 2;
|
|
default:
|
|
break;
|
|
}
|
|
printf("stmisp: No ACK to cmd 0x%02x (timeout)\n", cmd);
|
|
return 0;
|
|
}
|
|
|
|
int send_sync(stmisp_device_t *stmisp_device, uint32_t retries)
|
|
{
|
|
for(int i = 0; i < retries; i++)
|
|
{
|
|
uint8_t sync = SYNC_BYTE;
|
|
stmisp_device->send(&sync, 1, 0);
|
|
if(wait_ack(stmisp_device, 1000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_get(stmisp_device_t *stmisp_device)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, GET_CMD, 2000);
|
|
if(ack == 1)
|
|
{
|
|
stmisp_device->cmd_count = 0;
|
|
|
|
uint8_t size = stmisp_device->recv(&stmisp_device->cmd_count, 1, 1000);
|
|
size = stmisp_device->recv(&stmisp_device->version, 1, 1000);
|
|
printf("stmisp: version: %x cmd_count %d\n", stmisp_device->version, stmisp_device->cmd_count);
|
|
if(stmisp_device->cmd_count > 0 && size > 0)
|
|
{
|
|
size = stmisp_device->recv(stmisp_device->cmd_data, stmisp_device->cmd_count, 1000);
|
|
|
|
for(int i = 0; i < stmisp_device->cmd_count; i++)
|
|
{
|
|
printf(" %0x", stmisp_device->cmd_data[i]);
|
|
}
|
|
printf("\n");
|
|
if(wait_ack(stmisp_device, 1000) == 1){
|
|
return 1;
|
|
} else {
|
|
printf("stmisp: No ACK to cmd 0x%02x (timeout)\n", GET_CMD);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int cmd_getid(stmisp_device_t *stmisp_device)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, GETID_CMD, 1000);
|
|
|
|
if(ack == 1)
|
|
{
|
|
uint8_t size = stmisp_device->recv(&stmisp_device->num, 1, 1000);
|
|
size = stmisp_device->recv(&stmisp_device->pid, 2, 1000);
|
|
if(size == 2)
|
|
{
|
|
if(wait_ack(stmisp_device, 1000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_go(stmisp_device_t *stmisp_device, uint32_t adder)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, GO_CMD, 1000);
|
|
if(ack == 1)
|
|
{
|
|
uint8_t addrb[4] = { (uint8_t)(adder >> 24), (uint8_t)(adder >> 16), (uint8_t)(adder >> 8), (uint8_t)(adder >> 0) };
|
|
uint8_t cs_addr = xor_checksum(addrb, 4);
|
|
uint8_t pkt[5]; memcpy(pkt, addrb, 4); pkt[4] = cs_addr;
|
|
stmisp_device->send(pkt, 5, 0);
|
|
|
|
if(wait_ack(stmisp_device, 1000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_extended_erase_mass(stmisp_device_t *stmisp_device)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, ERASE_CMD, 1000);
|
|
if(ack == 1)
|
|
{
|
|
uint8_t payload[3];
|
|
payload[0] = 0xFF; payload[1] = 0xFF; payload[2] = xor_checksum(payload, 2);
|
|
stmisp_device->send(&payload, 3, 0);
|
|
if(wait_ack(stmisp_device, 60000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_write_unprotect(stmisp_device_t *stmisp_device)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, WRITE_UN_CMD, 1000);
|
|
if(ack == 1)
|
|
{
|
|
if(wait_ack(stmisp_device, 1000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_read_unprotect(stmisp_device_t *stmisp_device)
|
|
{
|
|
uint8_t ack = send_cmd_wait(stmisp_device, READ_UN_CMD, 1000);
|
|
if(ack == 1)
|
|
{
|
|
if(wait_ack(stmisp_device, 1000) == 1)
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int cmd_write_memory(stmisp_device_t *stmisp_device, uint32_t addr, const uint8_t *data, size_t len, bool verify)
|
|
{
|
|
size_t idx = 0;
|
|
while(idx < len)
|
|
{
|
|
size_t chunk = (len - idx) > 256 ? 256 : (len - idx);
|
|
|
|
uint8_t ack = send_cmd_wait(stmisp_device, WRITE_CMD, 1000);
|
|
if(ack == 1)
|
|
{
|
|
uint32_t a = addr + (uint32_t)idx;
|
|
uint8_t addrb[4] = { (uint8_t)(a >> 24), (uint8_t)(a >> 16), (uint8_t)(a >> 8), (uint8_t)(a >> 0) };
|
|
uint8_t cs_addr = xor_checksum(addrb, 4);
|
|
uint8_t pkt[5]; memcpy(pkt, addrb, 4); pkt[4] = cs_addr;
|
|
|
|
stmisp_device->send(pkt, 5, 0);
|
|
|
|
if(wait_ack(stmisp_device, 1000) != 1) return 0;
|
|
|
|
uint8_t nfield = (uint8_t)(chunk - 1);
|
|
|
|
uint8_t cs = nfield;
|
|
for (size_t i = 0; i < chunk; ++i) cs ^= data[idx + i];
|
|
|
|
stmisp_device->send(&nfield, 1, 0);
|
|
stmisp_device->send((void *)&data[idx], chunk, 0);
|
|
stmisp_device->send(&cs, 1, 0);
|
|
|
|
if(wait_ack(stmisp_device, 5000) != 1) return 0;
|
|
//是否需要校验写入的数据块
|
|
if (verify)
|
|
{
|
|
|
|
}
|
|
idx += chunk;
|
|
}
|
|
}
|
|
return 1;
|
|
}
|