369 lines
9.9 KiB
C
369 lines
9.9 KiB
C
/* common.c
|
|
*
|
|
* Copyright (C) 2021 wolfSSL Inc.
|
|
*
|
|
* This file is part of wolfSSL.
|
|
*
|
|
* wolfSSL is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* wolfSSL is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
|
|
*/
|
|
|
|
#include "common.h"
|
|
|
|
int sock;
|
|
volatile int keep_running = 1;
|
|
|
|
static uint8_t *copy_buf = NULL;
|
|
static uint8_t *copy_buf_ptr;
|
|
static size_t copy_buf_len;
|
|
static IsoTpLink g_link;
|
|
|
|
/* Alloc send and receive buffer statically in RAM */
|
|
static uint8_t g_isotpRecvBuf[ISOTP_BUFSIZE];
|
|
static uint8_t g_isotpSendBuf[ISOTP_BUFSIZE];
|
|
|
|
void sig_handle(int dummy);
|
|
|
|
/* Debug function required by ISO-TP to compile, can be blank */
|
|
void isotp_user_debug(const char* format, ...) {
|
|
va_list args;
|
|
va_start(args, format);
|
|
vprintf(format, args);
|
|
printf("\r\n");
|
|
va_end(args);
|
|
}
|
|
|
|
/* Timing function required by ISO-TP to compile */
|
|
uint32_t isotp_user_get_ms(void) {
|
|
struct timeval t;
|
|
gettimeofday(&t, NULL);
|
|
return (t.tv_sec * 1000) + (t.tv_usec / 1000);
|
|
}
|
|
|
|
/* CAN bus send function require by ISO-TP to compile */
|
|
int isotp_user_send_can(const uint32_t arbitration_id, const uint8_t* data,
|
|
const uint8_t size) {
|
|
/* Copy the message into a CAN bus frame, the message will always be
|
|
* 8 bytes or less */
|
|
struct can_frame frame;
|
|
frame.can_id = arbitration_id;
|
|
frame.can_dlc = size;
|
|
memcpy(frame.data, data, size);
|
|
|
|
/* Write the frame to the CAN bus */
|
|
if (write(sock, &frame, sizeof(struct can_frame))
|
|
!= sizeof(struct can_frame)) {
|
|
perror("Write error\n");
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/* Our CAN bus receive function */
|
|
int can_receive(uint8_t data[CAN_MSG_LEN], int *length) {
|
|
int nbytes;
|
|
struct can_frame frame;
|
|
struct pollfd p[1];
|
|
|
|
p[0].fd = sock;
|
|
p[0].events = POLLIN;
|
|
|
|
/* Poll for new data */
|
|
int retval = poll(p, 1, 10);
|
|
|
|
if (retval < 0) {
|
|
perror("Poll error\n");
|
|
return 1;
|
|
}
|
|
else if (retval == 0) {
|
|
/* No data */
|
|
*length = 0;
|
|
return EAGAIN;
|
|
}
|
|
|
|
/* Read in the frame data */
|
|
nbytes = read(sock, &frame, sizeof(struct can_frame));
|
|
if (nbytes < 0) {
|
|
perror("Read error\n");
|
|
return 1;
|
|
}
|
|
memcpy(data, frame.data, frame.can_dlc);
|
|
*length = frame.can_dlc;
|
|
return 0;
|
|
}
|
|
|
|
/* Connect to the CAN bus */
|
|
int can_connect(const char *address, uint16_t filter)
|
|
{
|
|
struct sockaddr_can addr;
|
|
struct ifreq ifr;
|
|
/* Setup a CAN bus ID filter so that the kernel space filters out messages
|
|
* we don't need. This is faster than userspace doing it */
|
|
struct can_filter rfilter[1];
|
|
rfilter[0].can_id = filter;
|
|
rfilter[0].can_mask = 0xFFF;
|
|
|
|
if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
|
|
perror("Socket open error\n");
|
|
return 1;
|
|
}
|
|
/* Set the filter */
|
|
setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
|
|
|
|
strcpy(ifr.ifr_name, address);
|
|
ioctl(sock, SIOCGIFINDEX, &ifr);
|
|
|
|
memset(&addr, 0, sizeof(addr));
|
|
addr.can_family = AF_CAN;
|
|
addr.can_ifindex = ifr.ifr_ifindex;
|
|
if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
|
perror("Bind error\n");
|
|
return 1;
|
|
}
|
|
|
|
/* Setup the receive copy buffer */
|
|
copy_buf = malloc(ISOTP_BUFSIZE);
|
|
copy_buf_ptr = copy_buf;
|
|
copy_buf_len = 0;
|
|
if (!copy_buf) {
|
|
fprintf(stderr, "Copy buf malloc fail\n");
|
|
return -99;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void can_close()
|
|
{
|
|
free(copy_buf);
|
|
close(sock);
|
|
}
|
|
|
|
/* The wolfSSL send packet callback. This function sends the data via ISO-TP
|
|
* and then listens and processes the ISO-TP control packets until the message
|
|
* is completely sent */
|
|
int send_ssl(WOLFSSL *ssl, char *buf, int sz, void *ctx)
|
|
{
|
|
uint8_t data[CAN_MSG_LEN];
|
|
int length;
|
|
IsoTpLink *g_link = (struct IsoTpLink*)ctx;
|
|
int ret = isotp_send(g_link, buf, sz);
|
|
printf("Sending %d bytes\n", sz);
|
|
|
|
if (ret) {
|
|
fprintf(stderr, "Error from isotp: %d\n", ret);
|
|
return -64;
|
|
}
|
|
|
|
while(g_link->send_status != ISOTP_SEND_STATUS_IDLE) {
|
|
ret = can_receive(data, &length);
|
|
if (!ret && (length > 0)) {
|
|
/* Got a control frame */
|
|
isotp_on_can_message(g_link, data, length);
|
|
} else if (ret != EAGAIN) {
|
|
fprintf(stderr, "Receive error\n");
|
|
}
|
|
isotp_poll(g_link);
|
|
}
|
|
|
|
return sz;
|
|
}
|
|
|
|
/* Receive callback for wolfSSL. ISO-TP will only return a buffer once and it
|
|
* can be more data than wolfSSL wants, so we copy this into an intermediate
|
|
* buffer and just return what wolfSSL is asking for */
|
|
int recv_ssl(WOLFSSL* ssl, char* buf, int sz, void* ctx)
|
|
{
|
|
uint8_t data[CAN_MSG_LEN];
|
|
int data_len;
|
|
uint16_t msg_len = 0;
|
|
int ret;
|
|
IsoTpLink *g_link = (struct IsoTpLink*)ctx;
|
|
|
|
if (!copy_buf_len) {
|
|
while (isotp_receive(g_link, copy_buf, ISOTP_BUFSIZE, &msg_len)
|
|
!= ISOTP_RET_OK) {
|
|
int ret = can_receive(data, &data_len);
|
|
if (ret == EAGAIN) {
|
|
return 0;
|
|
}
|
|
if (!ret && (data_len > 0)) {
|
|
isotp_on_can_message(g_link, data, data_len);
|
|
} else if (ret) {
|
|
fprintf(stderr, "Recieve error\n");
|
|
return -99;
|
|
}
|
|
}
|
|
if (msg_len) {
|
|
copy_buf_ptr = copy_buf;
|
|
printf("Receiving %d bytes\n", msg_len);
|
|
copy_buf_len = msg_len;
|
|
}
|
|
}
|
|
|
|
if (copy_buf_len >= sz) {
|
|
memcpy(buf, copy_buf_ptr, sz);
|
|
copy_buf_ptr+= sz;
|
|
copy_buf_len-= sz;
|
|
return sz;
|
|
} else {
|
|
memcpy(buf, copy_buf_ptr, copy_buf_len);
|
|
sz = copy_buf_len;
|
|
copy_buf_len = 0;
|
|
return sz;
|
|
}
|
|
|
|
return msg_len;
|
|
}
|
|
|
|
void close_ssl(WOLFSSL_CTX *ctx, WOLFSSL *ssl)
|
|
{
|
|
if (ssl) {
|
|
int ret = SSL_SHUTDOWN_NOT_DONE;
|
|
while (ret == SSL_SHUTDOWN_NOT_DONE) {
|
|
ret = wolfSSL_shutdown(ssl);
|
|
}
|
|
if (ret != SSL_SUCCESS) {
|
|
char buffer[ERR_MSG_LEN];
|
|
int err = wolfSSL_get_error(ssl, ret);
|
|
fprintf(stderr, "Error shutting down TLS connection: %d, %s",
|
|
err, wolfSSL_ERR_error_string(err, buffer));
|
|
return;
|
|
}
|
|
}
|
|
can_close();
|
|
|
|
wolfSSL_free(ssl);
|
|
wolfSSL_CTX_free(ctx);
|
|
}
|
|
|
|
void sig_handle(int dummy)
|
|
{
|
|
keep_running = 0;
|
|
}
|
|
|
|
int setup_connection(const char *interface, int local_id, int remote_id)
|
|
{
|
|
struct sigaction sa = { .sa_handler = sig_handle, /* .sa_flags = 0 */ };
|
|
sigaction(SIGINT, &sa, 0);
|
|
|
|
wolfSSL_Init();
|
|
|
|
/* Connect to CAN bus provided on command line, filter out everything
|
|
* except for the local CAN ID */
|
|
if (can_connect(interface, local_id)) {
|
|
return -1;
|
|
}
|
|
|
|
/* Setup ISO-TP and set the buffers */
|
|
isotp_init_link(&g_link, remote_id, g_isotpSendBuf,
|
|
sizeof(g_isotpSendBuf), g_isotpRecvBuf, sizeof(g_isotpRecvBuf));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int setup_ssl(enum service_type type, WOLFSSL_CTX **new_ctx,
|
|
WOLFSSL_METHOD **new_method, WOLFSSL **new_ssl)
|
|
{
|
|
int ret;
|
|
WOLFSSL_CTX *ctx = NULL;
|
|
WOLFSSL_METHOD* method = NULL;
|
|
WOLFSSL* ssl = NULL;
|
|
|
|
if (type == SERVICE_TYPE_CLIENT) {
|
|
method = wolfTLSv1_2_client_method();
|
|
} else {
|
|
method = wolfTLSv1_2_server_method();
|
|
}
|
|
|
|
if (!method) {
|
|
fprintf(stderr, "Could not init wolfSSL method\n");
|
|
return -1;
|
|
}
|
|
|
|
ctx = wolfSSL_CTX_new(method);
|
|
if (!ctx) {
|
|
fprintf(stderr, "Could not init wolfSSL context\n");
|
|
close_ssl(NULL, NULL);
|
|
return -1;
|
|
}
|
|
|
|
/* Set the send and receive callback functions which will use the
|
|
* CAN bus */
|
|
wolfSSL_CTX_SetIOSend(ctx, send_ssl);
|
|
wolfSSL_CTX_SetIORecv(ctx, recv_ssl);
|
|
|
|
wolfSSL_CTX_set_verify(ctx, WOLFSSL_VERIFY_NONE, NULL);
|
|
|
|
if (type == SERVICE_TYPE_CLIENT) {
|
|
ret = wolfSSL_CTX_load_verify_locations(ctx, "client.pem", NULL);
|
|
} else {
|
|
ret = wolfSSL_CTX_use_certificate_file(ctx, "server.pem",
|
|
SSL_FILETYPE_PEM);
|
|
}
|
|
|
|
if (ret != SSL_SUCCESS) {
|
|
fprintf(stderr, "ERROR: failed to load cert, "
|
|
"please check the file.\n");
|
|
close_ssl(ctx, NULL);
|
|
return -1;
|
|
}
|
|
|
|
if (type == SERVICE_TYPE_SERVER) {
|
|
if ((ret = wolfSSL_CTX_use_PrivateKey_file(ctx, "server.key",
|
|
SSL_FILETYPE_PEM)) != WOLFSSL_SUCCESS) {
|
|
fprintf(stderr, "ERROR: failed to load key file, "
|
|
"please check the file.\n");
|
|
close_ssl(ctx, NULL);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
ssl = wolfSSL_new(ctx);
|
|
if (!ssl) {
|
|
fprintf(stderr, "Could not init wolfSSL\n");
|
|
close_ssl(ctx, NULL);
|
|
return -1;
|
|
}
|
|
|
|
/* Set the read and write context to both use the ISO-TP link */
|
|
wolfSSL_SetIOWriteCtx(ssl, &g_link);
|
|
wolfSSL_SetIOReadCtx(ssl, &g_link);
|
|
|
|
if (type == SERVICE_TYPE_CLIENT) {
|
|
ret = wolfSSL_connect(ssl);
|
|
} else {
|
|
ret = wolfSSL_accept(ssl);
|
|
}
|
|
|
|
wolfSSL_set_using_nonblock(ssl, 1);
|
|
|
|
if (ret != SSL_SUCCESS) {
|
|
char buffer[ERR_MSG_LEN];
|
|
int err = wolfSSL_get_error(ssl, ret);
|
|
fprintf(stderr, "ERROR: failed to connect using wolfSSL: %d, %s\n",
|
|
err, wolfSSL_ERR_error_string(err, buffer));
|
|
close_ssl(ctx, ssl);
|
|
return -1;
|
|
}
|
|
*new_ctx = ctx;
|
|
*new_method = method;
|
|
*new_ssl = ssl;
|
|
|
|
printf("SSL handshake done!\n");
|
|
|
|
return 0;
|
|
}
|