More and more fixes.
This commit is contained in:
Pol Henarejos
parent
d4b0978d50
commit
da871e695e
6 changed files with 330 additions and 1147 deletions
1326
src/ccid/ccid2040.c
1326
src/ccid/ccid2040.c
File diff suppressed because it is too large
Load diff
|
|
@ -59,20 +59,16 @@ extern const uint8_t historical_bytes[];
|
|||
} printf("\r\n"); \
|
||||
}
|
||||
|
||||
|
||||
struct apdu {
|
||||
uint8_t seq;
|
||||
|
||||
/* command APDU */
|
||||
uint8_t *cmd_apdu_head; /* CLS INS P1 P2 [ internal Lc ] */
|
||||
uint8_t *cmd_apdu_data;
|
||||
size_t cmd_apdu_data_len; /* Nc, calculated by Lc field */
|
||||
size_t expected_res_size; /* Ne, calculated by Le field */
|
||||
|
||||
/* response APDU */
|
||||
uint16_t sw;
|
||||
uint16_t res_apdu_data_len;
|
||||
uint8_t *res_apdu_data;
|
||||
};
|
||||
uint8_t *header;
|
||||
uint32_t nc;
|
||||
uint32_t ne;
|
||||
uint8_t *data;
|
||||
uint16_t sw;
|
||||
uint8_t *rdata;
|
||||
uint16_t rlen;
|
||||
} __packed;
|
||||
|
||||
#define MAX_CMD_APDU_DATA_SIZE (24+4+512*4)
|
||||
#define MAX_RES_APDU_DATA_SIZE (5+9+512*4)
|
||||
|
|
@ -95,10 +91,6 @@ struct apdu {
|
|||
#define EV_BUTTON_TIMEOUT 16
|
||||
#define EV_BUTTON_PRESSED 32
|
||||
|
||||
//Variables set by core1
|
||||
extern queue_t *ccid_comm;
|
||||
extern queue_t *card_comm;
|
||||
|
||||
enum ccid_state {
|
||||
CCID_STATE_NOCARD, /* No card available */
|
||||
CCID_STATE_START, /* Initial */
|
||||
|
|
@ -112,13 +104,13 @@ enum ccid_state {
|
|||
CCID_STATE_EXEC_REQUESTED, /* Exec requested */
|
||||
};
|
||||
|
||||
#define CLA(a) a.cmd_apdu_head[0]
|
||||
#define INS(a) a.cmd_apdu_head[1]
|
||||
#define P1(a) a.cmd_apdu_head[2]
|
||||
#define P2(a) a.cmd_apdu_head[3]
|
||||
#define CLA(a) a.header[0]
|
||||
#define INS(a) a.header[1]
|
||||
#define P1(a) a.header[2]
|
||||
#define P2(a) a.header[3]
|
||||
|
||||
#define res_APDU apdu.res_apdu_data
|
||||
#define res_APDU_size apdu.res_apdu_data_len
|
||||
#define res_APDU apdu.rdata
|
||||
#define res_APDU_size apdu.rlen
|
||||
|
||||
extern struct apdu apdu;
|
||||
|
||||
|
|
@ -138,6 +130,9 @@ static inline const void put_uint16_t(uint16_t n, uint8_t *b) {
|
|||
|
||||
extern const uint8_t *ccid_atr;
|
||||
|
||||
extern queue_t ccid_to_card_q;
|
||||
extern queue_t card_to_ccid_q;
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
void stdout_init (void);
|
||||
|
|
|
|||
|
|
@ -88,13 +88,13 @@ int sm_unwrap() {
|
|||
return r;
|
||||
int le = sm_get_le();
|
||||
if (le >= 0)
|
||||
apdu.expected_res_size = le;
|
||||
apdu.ne = le;
|
||||
uint8_t *body = NULL;
|
||||
size_t body_size = 0;
|
||||
bool is87 = false;
|
||||
uint8_t tag = 0x0, *tag_data = NULL, *p = NULL;
|
||||
size_t tag_len = 0;
|
||||
while (walk_tlv(apdu.cmd_apdu_data, apdu.cmd_apdu_data_len, &p, &tag, &tag_len, &tag_data)) {
|
||||
while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) {
|
||||
if (tag == 0x87 || tag == 0x85) {
|
||||
body = tag_data;
|
||||
body_size = tag_len;
|
||||
|
|
@ -111,9 +111,9 @@ int sm_unwrap() {
|
|||
}
|
||||
sm_update_iv();
|
||||
aes_decrypt(sm_kenc, sm_iv, 128, HSM_AES_MODE_CBC, body, body_size);
|
||||
memmove(apdu.cmd_apdu_data, body, body_size);
|
||||
apdu.cmd_apdu_data_len = sm_remove_padding(apdu.cmd_apdu_data, body_size);
|
||||
DEBUG_PAYLOAD(apdu.cmd_apdu_data, apdu.cmd_apdu_data_len);
|
||||
memmove(apdu.data, body, body_size);
|
||||
apdu.nc = sm_remove_padding(apdu.data, body_size);
|
||||
DEBUG_PAYLOAD(apdu.data, apdu.nc);
|
||||
return CCID_OK;
|
||||
}
|
||||
|
||||
|
|
@ -173,15 +173,15 @@ int sm_wrap() {
|
|||
res_APDU[res_APDU_size++] = 0x8E;
|
||||
res_APDU[res_APDU_size++] = 8;
|
||||
res_APDU_size += 8;
|
||||
if (apdu.expected_res_size > 0)
|
||||
apdu.expected_res_size = res_APDU_size;
|
||||
if (apdu.ne > 0)
|
||||
apdu.ne = res_APDU_size;
|
||||
return CCID_OK;
|
||||
}
|
||||
|
||||
int sm_get_le() {
|
||||
uint8_t tag = 0x0, *tag_data = NULL, *p = NULL;
|
||||
size_t tag_len = 0;
|
||||
while (walk_tlv(apdu.cmd_apdu_data, apdu.cmd_apdu_data_len, &p, &tag, &tag_len, &tag_data)) {
|
||||
while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) {
|
||||
if (tag == 0x97) {
|
||||
uint32_t le = 0;
|
||||
for (int t = 1; t <= tag_len; t++)
|
||||
|
|
@ -205,7 +205,7 @@ int sm_verify() {
|
|||
memset(input, 0, sizeof(input));
|
||||
int input_len = 0, r = 0;
|
||||
bool add_header = (CLA(apdu) & 0xC) == 0xC;
|
||||
int data_len = (int)(apdu.cmd_apdu_data_len/sm_blocksize)*sm_blocksize;
|
||||
int data_len = (int)(apdu.nc/sm_blocksize)*sm_blocksize;
|
||||
if (data_len % sm_blocksize)
|
||||
data_len += sm_blocksize;
|
||||
if (data_len+(add_header ? sm_blocksize : 0) > 1024)
|
||||
|
|
@ -232,7 +232,7 @@ int sm_verify() {
|
|||
size_t mac_len = 0;
|
||||
uint8_t tag = 0x0, *tag_data = NULL, *p = NULL;
|
||||
size_t tag_len = 0;
|
||||
while (walk_tlv(apdu.cmd_apdu_data, apdu.cmd_apdu_data_len, &p, &tag, &tag_len, &tag_data)) {
|
||||
while (walk_tlv(apdu.data, apdu.nc, &p, &tag, &tag_len, &tag_data)) {
|
||||
if (tag & 0x1) {
|
||||
input[input_len++] = tag;
|
||||
int tlen = format_tlv_len(tag_len, input+input_len);
|
||||
|
|
|
|||
|
|
@ -121,7 +121,7 @@ file_t *get_parent(file_t *f) {
|
|||
|
||||
file_t *search_by_name(uint8_t *name, uint16_t namelen) {
|
||||
for (file_t *p = file_entries; p != file_last; p++) {
|
||||
if (p->name && *p->name == apdu.cmd_apdu_data_len && memcmp(p->name+1, name, namelen) == 0) {
|
||||
if (p->name && *p->name == apdu.nc && memcmp(p->name+1, name, namelen) == 0) {
|
||||
return p;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -149,7 +149,7 @@ static inline uint32_t usb_buffer_offset(volatile uint8_t *buf) {
|
|||
* @param ep
|
||||
*/
|
||||
void usb_setup_endpoint(const struct usb_endpoint_configuration *ep) {
|
||||
printf("Set up endpoint 0x%x with buffer address 0x%p\n", ep->descriptor->bEndpointAddress, ep->data_buffer);
|
||||
//printf("Set up endpoint 0x%x with buffer address 0x%p\n", ep->descriptor->bEndpointAddress, ep->data_buffer);
|
||||
|
||||
// EP0 doesn't have one so return if that is the case
|
||||
if (!ep->endpoint_control) {
|
||||
|
|
@ -243,7 +243,7 @@ void usb_start_transfer(struct usb_endpoint_configuration *ep, uint8_t *buf, uin
|
|||
// For multi packet transfers see the tinyusb port.
|
||||
assert(len <= 64);
|
||||
|
||||
printf("Start transfer of len %d on ep addr 0x%x\n", len, ep->descriptor->bEndpointAddress);
|
||||
//printf("Start transfer of len %d on ep addr 0x%x\n", len, ep->descriptor->bEndpointAddress);
|
||||
|
||||
// Prepare buffer control register value
|
||||
uint32_t val = len | USB_BUF_CTRL_AVAIL;
|
||||
|
|
@ -331,12 +331,6 @@ bool usb_is_configured(void) {
|
|||
return configured;
|
||||
}
|
||||
|
||||
uint32_t usb_write(uint8_t *buffer, size_t buffer_size) {
|
||||
struct usb_endpoint_configuration *ep = usb_get_endpoint_configuration(EP2_IN_ADDR);
|
||||
usb_start_transfer(ep, buffer, MIN(buffer_size, 64));
|
||||
return MIN(buffer_size, 64);
|
||||
}
|
||||
|
||||
void usb_init() {
|
||||
usb_device_init();
|
||||
|
||||
|
|
@ -491,7 +485,7 @@ static void usb_handle_ep_buff_done(struct usb_endpoint_configuration *ep) {
|
|||
*/
|
||||
static void usb_handle_buff_done(uint ep_num, bool in) {
|
||||
uint8_t ep_addr = ep_num | (in ? USB_DIR_IN : 0);
|
||||
printf("EP %d (in = %d) done\n", ep_num, in);
|
||||
//printf("EP %d (in = %d) done\n", ep_num, in);
|
||||
for (uint i = 0; i < USB_NUM_ENDPOINTS; i++) {
|
||||
struct usb_endpoint_configuration *ep = &dev_config.endpoints[i];
|
||||
if (ep->descriptor && ep->handler) {
|
||||
|
|
@ -551,7 +545,7 @@ void isr_usbctrl(void) {
|
|||
|
||||
// Bus is reset
|
||||
if (status & USB_INTS_BUS_RESET_BITS) {
|
||||
printf("BUS RESET\n");
|
||||
//printf("BUS RESET\n");
|
||||
handled |= USB_INTS_BUS_RESET_BITS;
|
||||
usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
|
||||
usb_bus_reset();
|
||||
|
|
@ -585,37 +579,72 @@ void ep0_out_handler(uint8_t *buf, uint16_t len) {
|
|||
;
|
||||
}
|
||||
|
||||
#define DEBUG_PAYLOAD(p,s) { \
|
||||
printf("Payload %s (%d bytes):\r\n", #p,s);\
|
||||
for (int i = 0; i < s; i += 16) {\
|
||||
printf("%07Xh : ",i+p);\
|
||||
for (int j = 0; j < 16; j++) {\
|
||||
if (j < s-i) printf("%02X ",(p)[i+j]);\
|
||||
else printf(" ");\
|
||||
if (j == 7) printf(" ");\
|
||||
} printf(": "); \
|
||||
for (int j = 0; j < MIN(16,s-i); j++) {\
|
||||
printf("%c",(p)[i+j] == 0x0a || (p)[i+j] == 0x0d ? '\\' : (p)[i+j]);\
|
||||
if (j == 7) printf(" ");\
|
||||
}\
|
||||
printf("\r\n");\
|
||||
} printf("\r\n"); \
|
||||
}
|
||||
|
||||
// Device specific functions
|
||||
static uint8_t rx_buffer[4096];
|
||||
static uint8_t rx_buffer[4096], tx_buffer[4096];
|
||||
static uint16_t w_offset = 0, r_offset = 0;
|
||||
|
||||
void ep1_out_handler(uint8_t *buf, uint16_t len) {
|
||||
printf("RX %d bytes from host\n", len);
|
||||
DEBUG_PAYLOAD(buf,len);
|
||||
// Send data back to host
|
||||
uint16_t size = MIN(sizeof(rx_buffer)-w_offset,len);
|
||||
if (size > 0) {
|
||||
memcpy(rx_buffer+w_offset, buf, size);
|
||||
w_offset += size;
|
||||
}
|
||||
/*
|
||||
do {
|
||||
uint16_t size = MIN(sizeof(rx_buffer)-w_offset,len);
|
||||
memcpy(rx_buffer+w_offset, buf, size);
|
||||
len -= size;
|
||||
w_offset += size;
|
||||
if (w_offset == sizeof(rx_buffer))
|
||||
w_offset = 0;
|
||||
buf += size;
|
||||
} while (len > 0);
|
||||
*/
|
||||
//printf("rx_w_offset %d, rx_r_offset %d\r\n",w_offset, r_offset);
|
||||
}
|
||||
|
||||
uint32_t usb_write(uint16_t size) {
|
||||
return usb_write_offset(size, 0);
|
||||
}
|
||||
|
||||
uint32_t usb_write_offset(uint16_t size, uint16_t roffset) {
|
||||
if (size > sizeof(tx_buffer))
|
||||
size = sizeof(tx_buffer);
|
||||
struct usb_endpoint_configuration *ep = usb_get_endpoint_configuration(EP2_IN_ADDR);
|
||||
for (uint16_t offset = roffset; offset-roffset < size; offset += 64) {
|
||||
usb_start_transfer(ep, tx_buffer+offset, MIN(size-offset, 64));
|
||||
}
|
||||
usb_start_transfer(usb_get_endpoint_configuration(EP1_OUT_ADDR), NULL, 64);
|
||||
return size;
|
||||
}
|
||||
uint16_t usb_read_available() {
|
||||
return w_offset - r_offset;
|
||||
}
|
||||
|
||||
uint8_t *usb_get_rx() {
|
||||
return rx_buffer;
|
||||
}
|
||||
uint8_t *usb_get_tx() {
|
||||
return tx_buffer;
|
||||
}
|
||||
|
||||
void usb_clear_rx() {
|
||||
w_offset = r_offset = 0;
|
||||
}
|
||||
|
||||
|
||||
uint16_t usb_read(uint8_t *buffer, size_t buffer_size) {
|
||||
if (w_offset-r_offset > 0) {
|
||||
uint16_t size = MIN(buffer_size, w_offset-r_offset);
|
||||
uint16_t size = MIN(buffer_size, w_offset-r_offset);
|
||||
if (size > 0) {
|
||||
memcpy(buffer, rx_buffer+r_offset, size);
|
||||
r_offset += size;
|
||||
if (r_offset == w_offset) {
|
||||
|
|
@ -627,9 +656,9 @@ uint16_t usb_read(uint8_t *buffer, size_t buffer_size) {
|
|||
}
|
||||
|
||||
void ep2_in_handler(uint8_t *buf, uint16_t len) {
|
||||
printf("Sent %d bytes to host\n", len);
|
||||
//printf("Sent %d bytes to host\n", len);
|
||||
// Get ready to rx again from host
|
||||
usb_start_transfer(usb_get_endpoint_configuration(EP1_OUT_ADDR), NULL, 64);
|
||||
//usb_start_transfer(usb_get_endpoint_configuration(EP1_OUT_ADDR), NULL, 64);
|
||||
}
|
||||
/*
|
||||
int main(void) {
|
||||
|
|
|
|||
|
|
@ -317,9 +317,14 @@ static const unsigned char *descriptor_strings[] = {
|
|||
|
||||
extern uint16_t usb_read(uint8_t *buffer, size_t buffer_size);
|
||||
extern uint16_t usb_read_available();
|
||||
extern uint32_t usb_write(uint8_t *buffer, size_t buffer_size);
|
||||
extern uint32_t usb_write_offset(uint16_t size, uint16_t offset);
|
||||
extern uint32_t usb_write(uint16_t size);
|
||||
extern bool usb_is_configured();
|
||||
extern void usb_init();
|
||||
extern uint8_t *usb_get_rx();
|
||||
extern uint32_t usb_send_tx_buffer();
|
||||
extern uint8_t *usb_get_tx();
|
||||
extern void usb_clear_rx();
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue