143 lines
3.5 KiB
C
143 lines
3.5 KiB
C
#include <string.h>
|
|
#include "apdu.h"
|
|
#include "error.h"
|
|
|
|
|
|
static inline void c_apdu_init(struct nsl_smartcard_c_apdu *apdu)
|
|
{
|
|
apdu->len = apdu->outlen = 0;
|
|
apdu->data = NULL;
|
|
}
|
|
|
|
static inline int c_apdu_read(struct nsl_smartcard_c_apdu *apdu, const uint8_t *data, size_t len)
|
|
{
|
|
size_t orig_len = len;
|
|
if (len < 4) {
|
|
return 4;
|
|
}
|
|
len -= 4;
|
|
apdu->class = *data++;
|
|
apdu->instruction = *data++;
|
|
apdu->params[0] = *data++;
|
|
apdu->params[1] = *data++;
|
|
|
|
/* figure out the amount of length bytes */
|
|
size_t len_len = 0;
|
|
if (!len) {
|
|
/* no length */
|
|
len_len = 0;
|
|
} else if (!*data && len >= 3) {
|
|
/* extended length */
|
|
data++;
|
|
len--;
|
|
len_len = 2;
|
|
} else {
|
|
/* short length */
|
|
len_len = 1;
|
|
}
|
|
|
|
if (len > len_len) {
|
|
/* we have Lc */
|
|
apdu->len = 0;
|
|
for (size_t i = 0; i < len_len; i++) {
|
|
apdu->len <<= 8;
|
|
apdu->len |= *data++;
|
|
}
|
|
len -= len_len;
|
|
|
|
if (len < apdu->len) {
|
|
return (orig_len - len) + apdu->len;
|
|
}
|
|
|
|
apdu->data = data;
|
|
data += apdu->len;
|
|
len -= apdu->len;
|
|
} else {
|
|
/* no Lc for us */
|
|
apdu->data = NULL;
|
|
apdu->len = 0;
|
|
}
|
|
|
|
if (len) {
|
|
/* we have Le */
|
|
if (len < len_len) {
|
|
return (orig_len - len) + len_len;
|
|
}
|
|
apdu->outlen = 0;
|
|
for (size_t i = 0; i < len_len; i++) {
|
|
apdu->outlen <<= 8;
|
|
apdu->outlen |= *data++;
|
|
}
|
|
if (!apdu->outlen) {
|
|
apdu->outlen = 1 << len_len;
|
|
}
|
|
len -= len_len;
|
|
} else {
|
|
/* no Le for us */
|
|
apdu->outlen = 0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static inline void r_apdu_init(struct nsl_smartcard_r_apdu *apdu)
|
|
{
|
|
apdu->status[0] = NSL_SMARTCARD_APDU_STATUS_ERROR_MISC;
|
|
apdu->status[1] = 0;
|
|
apdu->len = 0;
|
|
apdu->data = NULL;
|
|
}
|
|
|
|
static inline int r_apdu_write(struct nsl_smartcard_r_apdu *apdu, uint8_t *data, size_t *len)
|
|
{
|
|
if (*len < apdu->len + 2) {
|
|
return apdu->len + 2;
|
|
}
|
|
if (apdu->data) {
|
|
memcpy(data, apdu->data, apdu->len);
|
|
} else {
|
|
memset(data, 0, apdu->len);
|
|
}
|
|
data += apdu->len;
|
|
*data++ = apdu->status[0];
|
|
*data++ = apdu->status[1];
|
|
*len = apdu->len + 2;
|
|
return 0;
|
|
}
|
|
|
|
|
|
void nsl_smartcard_apdu_init(struct nsl_smartcard_apdu *apdu)
|
|
{
|
|
c_apdu_init(&apdu->command);
|
|
r_apdu_init(&apdu->response);
|
|
}
|
|
|
|
int nsl_smartcard_apdu_read_command(struct nsl_smartcard_apdu *apdu, const uint8_t *data, size_t len)
|
|
{
|
|
size_t needed = c_apdu_read(&apdu->command, data, len);
|
|
if (needed) {
|
|
apdu->response.status[0] = NSL_SMARTCARD_APDU_STATUS_ERROR_BAD_IN_LENGTH;
|
|
apdu->response.status[1] = needed;
|
|
apdu->response.len = 0;
|
|
return NSL_SMARTCARD_ERROR_WRONG_LENGTH;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int nsl_smartcard_apdu_write_response(struct nsl_smartcard_apdu *apdu, uint8_t *data, size_t *len)
|
|
{
|
|
if (apdu->response.len > apdu->command.outlen) {
|
|
apdu->response.status[0] = NSL_SMARTCARD_APDU_STATUS_ERROR_BAD_OUT_LENGTH;
|
|
apdu->response.status[1] = apdu->response.len;
|
|
apdu->response.len = 0;
|
|
}
|
|
size_t needed = r_apdu_write(&apdu->response, data, len);
|
|
if (needed) {
|
|
apdu->response.status[0] = NSL_SMARTCARD_APDU_STATUS_ERROR_BAD_OUT_LENGTH;
|
|
apdu->response.status[1] = needed;
|
|
apdu->response.len = 0;
|
|
r_apdu_write(&apdu->response, data, len);
|
|
}
|
|
return 0;
|
|
}
|