diff --git a/doc/dox_comments/header_files/wolfio.h b/doc/dox_comments/header_files/wolfio.h index 0c1f94e8d..0c7f8300f 100644 --- a/doc/dox_comments/header_files/wolfio.h +++ b/doc/dox_comments/header_files/wolfio.h @@ -537,3 +537,41 @@ WOLFSSL_API void wolfSSL_CTX_SetGenCookie(WOLFSSL_CTX*, CallbackGenCookie); \sa wolfSSL_CTX_SetGenCookie */ WOLFSSL_API void* wolfSSL_GetCookieCtx(WOLFSSL* ssl); + + +/*! + \ingroup Setup + + \brief This function sets up the ISO-TP context if wolfSSL, for use when + wolfSSL is compiled with WOLFSSL_ISOTP + + \return 0 on success, WOLFSSL_CBIO_ERR_GENERAL on failure + + \param ssl the wolfSSL context + \param ctx a user created ISOTP context which this function initializes + \param recv_fn a user CAN bus receive callback + \param send_fn a user CAN bus send callback + \param delay_fn a user microsecond granularity delay function + \param receive_delay a set amount of microseconds to delay each CAN bus + packet + \param receive_buffer a user supplied buffer to receive data, recommended + that is allocated to ISOTP_DEFAULT_BUFFER_SIZE bytes + \param receive_buffer_size - The size of receive_buffer + \param arg an arbitrary pointer sent to recv_fn and send_fn + + _Example_ + \code + struct can_info can_con_info; + isotp_wolfssl_ctx isotp_ctx; + char *receive_buffer = malloc(ISOTP_DEFAULT_BUFFER_SIZE); + WOLFSSL_CTX* ctx = wolfSSL_CTX_new(method); + WOLFSSL* ssl = wolfSSL_new(ctx); + ... + wolfSSL_SetIO_ISOTP(ssl, &isotp_ctx, can_receive, can_send, can_delay, 0, + receive_buffer, ISOTP_DEFAULT_BUFFER_SIZE, &can_con_info); + \endcode + */ +WOLFSSL_API int wolfSSL_SetIO_ISOTP(WOLFSSL *ssl, isotp_wolfssl_ctx *ctx, + can_recv_fn recv_fn, can_send_fn send_fn, can_delay_fn delay_fn, + word32 receive_delay, char *receive_buffer, int receive_buffer_size, + void *arg); diff --git a/src/internal.c b/src/internal.c index 0dded42a7..607f26773 100644 --- a/src/internal.c +++ b/src/internal.c @@ -2145,6 +2145,9 @@ int InitSSL_Ctx(WOLFSSL_CTX* ctx, WOLFSSL_METHOD* method, void* heap) #elif defined(WOLFSSL_GNRC) ctx->CBIORecv = GNRC_ReceiveFrom; ctx->CBIOSend = GNRC_SendTo; +#elif defined WOLFSSL_ISOTP + ctx->CBIORecv = ISOTP_Receive; + ctx->CBIOSend = ISOTP_Send; #endif #ifdef HAVE_PQC diff --git a/src/wolfio.c b/src/wolfio.c index f62fe782d..7fd72fb38 100644 --- a/src/wolfio.c +++ b/src/wolfio.c @@ -2714,4 +2714,388 @@ int wolfSSL_SetIO_LwIP(WOLFSSL* ssl, void* pcb, } #endif +#ifdef WOLFSSL_ISOTP +static int isotp_send_single_frame(struct isotp_wolfssl_ctx *ctx, char *buf, + word16 length) +{ + /* Length will be at most 7 bytes to get here. Packet is length and type + * for the first byte, then up to 7 bytes of data */ + ctx->frame.data[0] = ((byte)length) | (ISOTP_FRAME_TYPE_SINGLE << 4); + XMEMCPY(&ctx->frame.data[1], buf, length); + ctx->frame.length = length + 1; + return ctx->send_fn(&ctx->frame, ctx->arg); +} + +static int isotp_send_flow_control(struct isotp_wolfssl_ctx *ctx, + byte overflow) +{ + int ret; + /* Overflow is set it if we have been asked to receive more data than the + * user allocated a buffer for */ + if (overflow) { + ctx->frame.data[0] = ISOTP_FLOW_CONTROL_ABORT | + (ISOTP_FRAME_TYPE_CONTROL << 4); + } else { + ctx->frame.data[0] = ISOTP_FLOW_CONTROL_CTS | + (ISOTP_FRAME_TYPE_CONTROL << 4); + } + /* Set the number of frames between flow control to infinite */ + ctx->frame.data[1] = ISOTP_FLOW_CONTROL_FRAMES; + /* User specified frame delay */ + ctx->frame.data[2] = ctx->receive_delay; + ctx->frame.length = ISOTP_FLOW_CONTROL_PACKET_SIZE; + ret = ctx->send_fn(&ctx->frame, ctx->arg); + return ret; +} + +static int isotp_receive_flow_control(struct isotp_wolfssl_ctx *ctx) +{ + int ret; + enum isotp_frame_type type; + enum isotp_flow_control flow_control; + ret = ctx->recv_fn(&ctx->frame, ctx->arg, ISOTP_DEFAULT_TIMEOUT); + if (ret == 0) { + return WOLFSSL_CBIO_ERR_TIMEOUT; + } else if (ret < 0) { + WOLFSSL_MSG("ISO-TP error receiving flow control packet"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + /* Flow control is the frame type and flow response for the first byte, + * number of frames until the next flow control packet for the second + * byte, time between frames for the third byte */ + type = ctx->frame.data[0] >> 4; + + if (type != ISOTP_FRAME_TYPE_CONTROL) { + WOLFSSL_MSG("ISO-TP frames out of sequence"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + + flow_control = ctx->frame.data[0] & 0xf; + + ctx->flow_counter = 0; + ctx->flow_packets = ctx->frame.data[1]; + ctx->frame_delay = ctx->frame.data[2]; + + return flow_control; +} + +static int isotp_send_consecutive_frame(struct isotp_wolfssl_ctx *ctx) +{ + /* Sequence is 0 - 15 and then starts again, the first frame has an + * implied sequence of '0' */ + ctx->sequence += 1; + if (ctx->sequence > ISOTP_MAX_SEQUENCE_COUNTER) { + ctx->sequence = 0; + } + ctx->flow_counter++; + /* First byte it type and sequence number, up to 7 bytes of data */ + ctx->frame.data[0] = ctx->sequence | (ISOTP_FRAME_TYPE_CONSECUTIVE << 4); + if (ctx->buf_length > ISOTP_MAX_CONSECUTIVE_FRAME_DATA_SIZE) { + XMEMCPY(&ctx->frame.data[1], ctx->buf_ptr, + ISOTP_MAX_CONSECUTIVE_FRAME_DATA_SIZE); + ctx->buf_ptr += ISOTP_MAX_CONSECUTIVE_FRAME_DATA_SIZE; + ctx->buf_length -= ISOTP_MAX_CONSECUTIVE_FRAME_DATA_SIZE; + ctx->frame.length = ISOTP_CAN_BUS_PAYLOAD_SIZE; + } else { + XMEMCPY(&ctx->frame.data[1], ctx->buf_ptr, ctx->buf_length); + ctx->frame.length = ctx->buf_length + 1; + ctx->buf_length = 0; + } + return ctx->send_fn(&ctx->frame, ctx->arg); + +} + +static int isotp_send_first_frame(struct isotp_wolfssl_ctx *ctx, char *buf, + word16 length) +{ + int ret; + ctx->sequence = 0; + /* Set to 1 to trigger a flow control straight away, the flow control + * packet will set these properly */ + ctx->flow_packets = ctx->flow_counter = 1; + /* First frame has 1 nibble for type, 3 nibbles for length followed by + * 6 bytes for data*/ + ctx->frame.data[0] = (length >> 8) | (ISOTP_FRAME_TYPE_FIRST << 4); + ctx->frame.data[1] = length & 0xff; + XMEMCPY(&ctx->frame.data[2], buf, ISOTP_FIRST_FRAME_DATA_SIZE); + ctx->buf_ptr = buf + ISOTP_FIRST_FRAME_DATA_SIZE; + ctx->buf_length = length - ISOTP_FIRST_FRAME_DATA_SIZE; + ctx->frame.length = ISOTP_CAN_BUS_PAYLOAD_SIZE; + ret = ctx->send_fn(&ctx->frame, ctx->arg); + if (ret <= 0) { + WOLFSSL_MSG("ISO-TP error sending first frame"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + while(ctx->buf_length) { + /* The receiver can set how often to get a flow control packet. If it + * is time, then get the packet. Note that this will always happen + * after the first packet */ + if ((ctx->flow_packets > 0) && + (ctx->flow_counter == ctx->flow_packets)) { + ret = isotp_receive_flow_control(ctx); + } + /* Frame delay <= 0x7f is in ms, 0xfX is X * 100 us */ + if (ctx->frame_delay) { + if (ctx->frame_delay <= ISOTP_MAX_MS_FRAME_DELAY) { + ctx->delay_fn(ctx->frame_delay * 1000); + } else { + ctx->delay_fn((ctx->frame_delay & 0xf) * 100); + } + } + switch (ret) { + /* Clear to send */ + case ISOTP_FLOW_CONTROL_CTS: + if (isotp_send_consecutive_frame(ctx) < 0) { + WOLFSSL_MSG("ISO-TP error sending consecutive frame"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + break; + /* Receiver says "WAIT", so we wait for another flow control + * packet, or abort if we have waited too long */ + case ISOTP_FLOW_CONTROL_WAIT: + ctx->wait_counter += 1; + if (ctx->wait_counter > ISOTP_DEFAULT_WAIT_COUNT) { + WOLFSSL_MSG("ISO-TP receiver told us to wait too many" + " times"); + return WOLFSSL_CBIO_ERR_WANT_WRITE; + } + break; + /* Receiver is not ready to receive packet, so abort */ + case ISOTP_FLOW_CONTROL_ABORT: + WOLFSSL_MSG("ISO-TP receiver aborted transmission"); + return WOLFSSL_CBIO_ERR_WANT_WRITE; + default: + WOLFSSL_MSG("ISO-TP got unexpected flow control packet"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + } + return 0; +} + +int ISOTP_Send(WOLFSSL* ssl, char* buf, int sz, void* ctx) +{ + int ret; + struct isotp_wolfssl_ctx *isotp_ctx; + (void) ssl; + + if (!ctx) { + WOLFSSL_MSG("ISO-TP requires wolfSSL_SetIO_ISOTP to be called first"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + isotp_ctx = (struct isotp_wolfssl_ctx*) ctx; + + /* ISO-TP cannot send more than 4095 bytes, this limits the packet size + * and wolfSSL will try again with the remaining data */ + if (sz > ISOTP_MAX_DATA_SIZE) { + sz = ISOTP_MAX_DATA_SIZE; + } + /* Can't send whilst we are receiving */ + if (isotp_ctx->state != ISOTP_CONN_STATE_IDLE) { + return WOLFSSL_ERROR_WANT_WRITE; + } + isotp_ctx->state = ISOTP_CONN_STATE_SENDING; + + /* Assuming normal addressing */ + if (sz <= ISOTP_SINGLE_FRAME_DATA_SIZE) { + ret = isotp_send_single_frame(isotp_ctx, buf, (word16)sz); + } else { + ret = isotp_send_first_frame(isotp_ctx, buf, (word16)sz); + } + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + + if (ret == 0) { + return sz; + } + return ret; +} + +static int isotp_receive_single_frame(struct isotp_wolfssl_ctx *ctx) +{ + byte data_size; + + /* 1 nibble for data size which will be 1 - 7 in a regular 8 byte CAN + * packet */ + data_size = (byte)ctx->frame.data[0] & 0xf; + if (ctx->receive_buffer_size < (int)data_size) { + WOLFSSL_MSG("ISO-TP buffer is too small to receive data"); + return BUFFER_E; + } + XMEMCPY(ctx->receive_buffer, &ctx->frame.data[1], data_size); + return data_size; +} + +static int isotp_receive_multi_frame(struct isotp_wolfssl_ctx *ctx) +{ + int ret; + word16 data_size; + byte delay = 0; + + /* Increase receive timeout for enforced ms delay */ + if (ctx->receive_delay <= ISOTP_MAX_MS_FRAME_DELAY) { + delay = ctx->receive_delay; + } + /* Still processing first frame. + * Full data size is lower nibble of first byte for the most significant + * followed by the second byte for the rest. Last 6 bytes are data */ + data_size = ((ctx->frame.data[0] & 0xf) << 8) + ctx->frame.data[1]; + XMEMCPY(ctx->receive_buffer, &ctx->frame.data[2], ISOTP_FIRST_FRAME_DATA_SIZE); + /* Need to send a flow control packet to either cancel or continue + * transmission of data */ + if (ctx->receive_buffer_size < data_size) { + isotp_send_flow_control(ctx, TRUE); + WOLFSSL_MSG("ISO-TP buffer is too small to receive data"); + return BUFFER_E; + } + isotp_send_flow_control(ctx, FALSE); + + ctx->buf_length = ISOTP_FIRST_FRAME_DATA_SIZE; + ctx->buf_ptr = ctx->receive_buffer + ISOTP_FIRST_FRAME_DATA_SIZE; + data_size -= ISOTP_FIRST_FRAME_DATA_SIZE; + ctx->sequence = 1; + + while(data_size) { + enum isotp_frame_type type; + byte sequence; + byte frame_len; + ret = ctx->recv_fn(&ctx->frame, ctx->arg, ISOTP_DEFAULT_TIMEOUT + + (delay / 1000)); + if (ret == 0) { + return WOLFSSL_CBIO_ERR_TIMEOUT; + } + type = ctx->frame.data[0] >> 4; + /* Consecutive frames have sequence number as lower nibble */ + sequence = ctx->frame.data[0] & 0xf; + if (type != ISOTP_FRAME_TYPE_CONSECUTIVE) { + WOLFSSL_MSG("ISO-TP frames out of sequence"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + if (sequence != ctx->sequence) { + WOLFSSL_MSG("ISO-TP frames out of sequence"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + /* Last 7 bytes or whatever we got after the first byte is data */ + frame_len = ctx->frame.length - 1; + XMEMCPY(ctx->buf_ptr, &ctx->frame.data[1], frame_len); + ctx->buf_ptr += frame_len; + ctx->buf_length += frame_len; + data_size -= frame_len; + + /* Sequence is 0 - 15 (first 0 is implied for first packet */ + ctx->sequence++; + if (ctx->sequence > ISOTP_MAX_SEQUENCE_COUNTER) { + ctx->sequence = 0; + } + } + return ctx->buf_length; + +} + +/* The wolfSSL receive callback, needs to buffer because we need to grab all + * incoming data, even if wolfSSL doesn't want it all yet */ +int ISOTP_Receive(WOLFSSL* ssl, char* buf, int sz, void* ctx) +{ + enum isotp_frame_type type; + int ret; + struct isotp_wolfssl_ctx *isotp_ctx; + (void) ssl; + + if (!ctx) { + WOLFSSL_MSG("ISO-TP requires wolfSSL_SetIO_ISOTP to be called first"); + return WOLFSSL_CBIO_ERR_TIMEOUT; + } + isotp_ctx = (struct isotp_wolfssl_ctx*)ctx; + + /* Is buffer empty? If so, fill it */ + if (!isotp_ctx->receive_buffer_len) { + /* Can't send whilst we are receiving */ + if (isotp_ctx->state != ISOTP_CONN_STATE_IDLE) { + return WOLFSSL_ERROR_WANT_READ; + } + isotp_ctx->state = ISOTP_CONN_STATE_RECEIVING; + do { + ret = isotp_ctx->recv_fn(&isotp_ctx->frame, isotp_ctx->arg, + ISOTP_DEFAULT_TIMEOUT); + } while (ret == 0); + if (ret == 0) { + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + return WOLFSSL_CBIO_ERR_TIMEOUT; + } else if (ret < 0) { + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + WOLFSSL_MSG("ISO-TP receive error"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + + type = (enum isotp_frame_type) isotp_ctx->frame.data[0] >> 4; + + if (type == ISOTP_FRAME_TYPE_SINGLE) { + isotp_ctx->receive_buffer_len = + isotp_receive_single_frame(isotp_ctx); + } else if (type == ISOTP_FRAME_TYPE_FIRST) { + isotp_ctx->receive_buffer_len = + isotp_receive_multi_frame(isotp_ctx); + } else { + /* Should never get here */ + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + WOLFSSL_MSG("ISO-TP frames out of sequence"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + if (isotp_ctx->receive_buffer_len <= 1) { + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + return isotp_ctx->receive_buffer_len; + } else { + isotp_ctx->receive_buffer_ptr = isotp_ctx->receive_buffer; + } + isotp_ctx->state = ISOTP_CONN_STATE_IDLE; + } + + /* Return from the buffer */ + if (isotp_ctx->receive_buffer_len >= sz) { + XMEMCPY(buf, isotp_ctx->receive_buffer_ptr, sz); + isotp_ctx->receive_buffer_ptr+= sz; + isotp_ctx->receive_buffer_len-= sz; + return sz; + } else { + XMEMCPY(buf, isotp_ctx->receive_buffer_ptr, + isotp_ctx->receive_buffer_len); + sz = isotp_ctx->receive_buffer_len; + isotp_ctx->receive_buffer_len = 0; + return sz; + } +} + +int wolfSSL_SetIO_ISOTP(WOLFSSL *ssl, isotp_wolfssl_ctx *ctx, + can_recv_fn recv_fn, can_send_fn send_fn, can_delay_fn delay_fn, + word32 receive_delay, char *receive_buffer, int receive_buffer_size, + void *arg) +{ + if (!ctx || !recv_fn || !send_fn || !delay_fn || !receive_buffer) { + WOLFSSL_MSG("ISO-TP has missing required parameter"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + ctx->recv_fn = recv_fn; + ctx->send_fn = send_fn; + ctx->arg = arg; + ctx->delay_fn = delay_fn; + ctx->frame_delay = 0; + ctx->receive_buffer = receive_buffer; + ctx->receive_buffer_size = receive_buffer_size; + ctx->receive_buffer_len = 0; + ctx->state = ISOTP_CONN_STATE_IDLE; + + wolfSSL_SetIOReadCtx(ssl, ctx); + wolfSSL_SetIOWriteCtx(ssl, ctx); + + /* Delay of 100 - 900us is 0xfX where X is value / 100. Delay of + * >= 1000 is divided by 1000. > 127ms is invalid */ + if (receive_delay < 1000) { + ctx->receive_delay = 0xf0 + (receive_delay / 100); + } else if (receive_delay <= ISOTP_MAX_MS_FRAME_DELAY * 1000) { + ctx->receive_delay = receive_delay / 1000; + } else { + WOLFSSL_MSG("ISO-TP delay parameter out of bounds"); + return WOLFSSL_CBIO_ERR_GENERAL; + } + return 0; +} +#endif #endif /* WOLFCRYPT_ONLY */ diff --git a/wolfssl/wolfio.h b/wolfssl/wolfio.h index d2a22254c..c3a1bc673 100644 --- a/wolfssl/wolfio.h +++ b/wolfssl/wolfio.h @@ -615,7 +615,82 @@ WOLFSSL_API void wolfSSL_SetIOWriteFlags(WOLFSSL* ssl, int flags); WOLFSSL_API int wolfSSL_SetIO_LwIP(WOLFSSL* ssl, void *pcb, tcp_recv_fn recv, tcp_sent_fn sent, void *arg); #endif +#ifdef WOLFSSL_ISOTP + #define ISOTP_DEFAULT_TIMEOUT 100 + #define ISOTP_DEFAULT_WAIT_COUNT 3 + #define ISOTP_FIRST_FRAME_DATA_SIZE 6 + #define ISOTP_SINGLE_FRAME_DATA_SIZE 7 + #define ISOTP_MAX_CONSECUTIVE_FRAME_DATA_SIZE 7 + #define ISOTP_MAX_MS_FRAME_DELAY 0x7f + #define ISOTP_CAN_BUS_PAYLOAD_SIZE 8 + #define ISOTP_MAX_DATA_SIZE 4095 + /* Packets will never be larger than the ISO-TP max data size */ + #define ISOTP_DEFAULT_BUFFER_SIZE ISOTP_MAX_DATA_SIZE + #define ISOTP_FLOW_CONTROL_PACKET_SIZE 3 + #define ISOTP_FLOW_CONTROL_FRAMES 0 /* infinite */ + #define ISOTP_MAX_SEQUENCE_COUNTER 15 + enum isotp_frame_type { + ISOTP_FRAME_TYPE_SINGLE = 0, + ISOTP_FRAME_TYPE_FIRST = 1, + ISOTP_FRAME_TYPE_CONSECUTIVE = 2, + ISOTP_FRAME_TYPE_CONTROL = 3 + }; + + enum isotp_flow_control { + ISOTP_FLOW_CONTROL_CTS = 0, + ISOTP_FLOW_CONTROL_WAIT = 1, + ISOTP_FLOW_CONTROL_ABORT = 2 + }; + + enum isotp_connection_state { + ISOTP_CONN_STATE_IDLE, + ISOTP_CONN_STATE_SENDING, + ISOTP_CONN_STATE_RECEIVING + }; + + typedef struct isotp_can_data { + byte data[ISOTP_CAN_BUS_PAYLOAD_SIZE]; + byte length; + } isotp_can_data; + + /* User supplied functions for sending/receiving CAN bus messages of up to + * 8 bytes, as well as a function to add an artificial delay when a + * receiver requests one. */ + typedef int (*can_recv_fn)(struct isotp_can_data *data, void *arg, + int timeout); + typedef int (*can_send_fn)(struct isotp_can_data *data, void *arg); + typedef void (*can_delay_fn)(int microseconds); + + typedef struct isotp_wolfssl_ctx { + struct isotp_can_data frame; + char *buf_ptr; + char *receive_buffer; + char *receive_buffer_ptr; + can_recv_fn recv_fn; + can_send_fn send_fn; + can_delay_fn delay_fn; + void *arg; + int receive_buffer_len; + int receive_buffer_size; + enum isotp_connection_state state; + word16 buf_length; + byte sequence; + byte flow_packets; + byte flow_counter; + byte frame_delay; + byte wait_counter; + byte receive_delay; + } isotp_wolfssl_ctx; + + WOLFSSL_LOCAL int ISOTP_Receive(WOLFSSL* ssl, char* buf, int sz, void* ctx); + WOLFSSL_LOCAL int ISOTP_Send(WOLFSSL* ssl, char* buf, int sz, void* ctx); + WOLFSSL_API int wolfSSL_SetIO_ISOTP(WOLFSSL *ssl, isotp_wolfssl_ctx *ctx, + can_recv_fn recv_fn, can_send_fn send_fn, can_delay_fn delay_fn, + word32 receive_delay, char *receive_buffer, + int receive_buffer_size, void *arg); + +#endif #ifdef WOLFSSL_DTLS typedef int (*CallbackGenCookie)(WOLFSSL* ssl, unsigned char* buf, int sz, void* ctx);