GArray*
g_array_insert_vals (GArray *array,
guint index_,
gconstpointer data,
guint len)
{
GArrayPriv *priv = (GArrayPriv*)array;
guint extra = (priv->zero_terminated ? 1 : 0);
g_return_val_if_fail (array != NULL, NULL);
ensure_capacity (priv, array->len + len + extra);
/* first move the existing elements out of the way */
memmove (element_offset (priv, index_ + len),
element_offset (priv, index_),
element_length (priv, array->len - index_));
/* then copy the new elements into the array */
memmove (element_offset (priv, index_),
data,
element_length (priv, len));
array->len += len;
if (priv->zero_terminated) {
memset (element_offset (priv, priv->array.len),
0,
priv->element_size);
}
return array;
}
status_t element_to_key(element_t e, uint8_t *data, int data_len, uint8_t label)
{
LEAVE_IF(e->isInitialized != TRUE, "uninitialized argument.");
// adds an extra null byte by default - will use this last byte for the label
int d_len = element_length(e), digest_len = SHA_LEN;
uint8_t d[d_len + 1];
memset(d, 0, d_len);
// write e to a tmp buf
if(d_len > 0 && digest_len <= data_len) {
element_to_bytes(d, d_len, e);
d[d_len-1] = label;
#ifdef DEBUG
printf("%s: bytes form....\n", __FUNCTION__);
print_as_hex(d, d_len);
#endif
// hash buf using md_map_sh256 and store data_len bytes in data
uint8_t digest[digest_len + 1];
memset(digest, 0, digest_len);
SHA_FUNC(digest, d, d_len);
memcpy(data, digest, digest_len);
#ifdef DEBUG
printf("%s: digest: ", __FUNCTION__);
print_as_hex(data, digest_len);
#endif
return ELEMENT_OK;
}
return ELEMENT_INVALID_ARG;
}
GArray *
g_array_append_vals (GArray *array,
gconstpointer data,
guint len)
{
GArrayPriv *priv = (GArrayPriv*)array;
g_return_val_if_fail (array != NULL, NULL);
ensure_capacity (priv, priv->array.len + len + (priv->zero_terminated ? 1 : 0));
memmove (element_offset (priv, priv->array.len),
data,
element_length (priv, len));
priv->array.len += len;
if (priv->zero_terminated) {
memset (element_offset (priv, priv->array.len),
0,
priv->element_size);
}
return array;
}
void print_array_of_strings(char **a)
{
int i;
int j;
int elements = element_length(a);
char *str;
i = 0;
j = 0;
str = malloc (elements * sizeof(char));
while (a[i] != '\0')
{
while (a[i][j] != '\0')
{
str[j] = a[i][j];
print_char(str[j]);
j++;
}
j = 0;
print_char(' ');
i++;
}
print_char('\n');
free(str);
}
static void
ensure_capacity (GArrayPriv *priv, guint capacity)
{
guint new_capacity;
if (capacity <= priv->capacity)
return;
new_capacity = (capacity + 63) & ~63;
priv->array.data = g_realloc (priv->array.data, element_length (priv, new_capacity));
if (priv->clear_) {
memset (element_offset (priv, priv->capacity),
0,
element_length (priv, new_capacity - priv->capacity));
}
priv->capacity = new_capacity;
}
static void
ensure_capacity (GArrayPriv *priv,
int capacity)
{
int new_capacity = MAX (priv->capacity, INITIAL_CAPACITY);
if (capacity < priv->capacity)
return;
while (new_capacity < capacity) {
new_capacity <<= 1;
}
capacity = new_capacity;
priv->array.data = (gchar*)g_realloc (priv->array.data, element_length (priv, capacity));
if (priv->clear_) {
memset (element_offset (priv, priv->capacity),
0,
element_length (priv, capacity - priv->capacity));
}
priv->capacity = capacity;
}
//TODO:
status_t element_to_str(char *data, int len, element_t e)
{
if(e->isInitialized == TRUE) {
int str_len = element_length(e) * 2;
if(str_len > len) return ELEMENT_INVALID_ARG;
memset(data, 0, len);
uint8_t tmp1[str_len+1];
memset(tmp1, 0, str_len);
if(e->type == ZR) {
bn_write_str(data, str_len, e->bn, BASE);
}
else if(e->type == G1) {
g1_write_str(e->g1, tmp1, str_len);
int dist_y = FP_STR;
snprintf(data, len, "[%s, %s]", tmp1, &(tmp1[dist_y]));
}
else if(e->type == G2) {
g2_write_str(e->g2, tmp1, str_len);
int len2 = FP_STR;
int dist_x1 = len2, dist_y0 = len2 * 2, dist_y1 = len2 * 3;
snprintf(data, len, "[%s, %s, %s, %s]", tmp1, &(tmp1[dist_x1]), &(tmp1[dist_y0]), &(tmp1[dist_y1]));
}
else if(e->type == GT) {
gt_write_str(e->gt, tmp1, str_len);
int len2 = FP_STR;
int dist_x01 = len2, dist_x10 = len2 * 2, dist_x11 = len2 * 3,
dist_x20 = len2 * 4, dist_x21 = len2 * 5, dist_y00 = len2 * 6,
dist_y01 = len2 * 7, dist_y10 = len2 * 8, dist_y11 = len2 * 9,
dist_y20 = len2 * 10, dist_y21 = len2 * 11;
snprintf(data, len, "[%s, %s, %s, %s, %s, %s], [%s, %s, %s, %s, %s, %s]",
tmp1, &(tmp1[dist_x01]), &(tmp1[dist_x10]), &(tmp1[dist_x11]),
&(tmp1[dist_x20]), &(tmp1[dist_x21]),
&(tmp1[dist_y00]), &(tmp1[dist_y01]), &(tmp1[dist_y10]), &(tmp1[dist_y11]),
&(tmp1[dist_y20]), &(tmp1[dist_y21]));
}
}
return ELEMENT_OK;
}
GArray*
g_array_remove_index_fast (GArray *array,
guint index_)
{
GArrayPriv *priv = (GArrayPriv*)array;
g_return_val_if_fail (array != NULL, NULL);
memmove (element_offset (priv, index_),
element_offset (priv, array->len - 1),
element_length (priv, 1));
array->len --;
if (priv->zero_terminated) {
memset (element_offset (priv, priv->array.len),
0,
priv->element_size);
}
return array;
}
void state_isotp() {
int i, items, ret;
char rxmsg[MAXLEN]; /* can to inet */
char buf[MAXLEN]; /* inet commands to can */
unsigned char isobuf[ISOTPLEN+1]; /* binary buffer for isotp socket */
unsigned char tmp;
if(previous_state == STATE_ISOTP) {
state_isotp_init();
previous_state = STATE_ISOTP;
}
FD_ZERO(&readfds);
FD_SET(si, &readfds);
FD_SET(client_socket, &readfds);
/*
* Check if there are more elements in the element buffer before calling select() and
* blocking for new packets.
*/
if(more_elements) {
FD_CLR(si, &readfds);
} else {
ret = select((si > client_socket)?si+1:client_socket+1, &readfds, NULL, NULL, NULL);
if(ret < 0) {
PRINT_ERROR("Error in select()\n")
change_state(STATE_SHUTDOWN);
return;
}
}
if (FD_ISSET(si, &readfds)) {
struct timeval tv = {0};
items = read(si, isobuf, ISOTPLEN);
/* read timestamp data */
if(ioctl(si, SIOCGSTAMP, &tv) < 0) {
PRINT_ERROR("Could not receive timestamp\n");
}
if (items > 0 && items <= ISOTPLEN) {
int startlen;
sprintf(rxmsg, "< pdu %ld.%06ld ", tv.tv_sec, tv.tv_usec);
startlen = strlen(rxmsg);
for (i=0; i < items; i++)
sprintf(rxmsg + startlen + 2*i, "%02X", isobuf[i]);
sprintf(rxmsg + strlen(rxmsg), " >");
send(client_socket, rxmsg, strlen(rxmsg), 0);
}
}
if (FD_ISSET(client_socket, &readfds)) {
ret = receive_command(client_socket, buf);
if(ret != 0) {
change_state(STATE_SHUTDOWN);
return;
}
if ( (ret = state_changed(buf, state)) ) {
if(ret == CONTROL_SWITCH_STATE) state_isotp_deinit();
strcpy(buf, "< ok >");
send(client_socket, buf, strlen(buf), 0);
return;
}
#if 0
if(!strcmp("< echo >", buf)) {
send(client_socket, buf, strlen(buf), 0);
return;
}
if(!strncmp("< sendpdu ", buf, 10)) {
items = element_length(buf, 2);
if (items & 1) {
PRINT_ERROR("odd number of ASCII Hex values\n");
return;
}
items /= 2;
if (items > ISOTPLEN) {
PRINT_ERROR("PDU too long\n");
return;
}
for (i = 0; i < items; i++) {
tmp = asc2nibble(buf[(2*i) + 10]);
if (tmp > 0x0F)
return;
isobuf[i] = (tmp << 4);
tmp = asc2nibble(buf[(2*i) + 11]);
if (tmp > 0x0F)
return;
isobuf[i] |= tmp;
}
ret = write(si, isobuf, items);
if(ret != items) {
PRINT_ERROR("Error in write()\n")
change_state(STATE_SHUTDOWN);
return;
}
} else {
PRINT_ERROR("unknown command '%s'.\n", buf)
strcpy(buf, "< error unknown command >");
send(client_socket, buf, strlen(buf), 0);
}
#else
ret = decode_command(buf);
switch(ret) {
case COMMAND_ECHO:
send(client_socket, buf, strlen(buf), 0);
break;
case COMMAND_SENDPDU:
items = element_length(buf, 2);
if (items & 1) {
PRINT_ERROR("odd number of ASCII Hex values\n");
return;
}
items /= 2;
if (items > ISOTPLEN) {
PRINT_ERROR("PDU too long\n");
return;
}
for (i = 0; i < items; i++) {
tmp = asc2nibble(buf[(2*i) + 10]);
if (tmp > 0x0F)
return;
isobuf[i] = (tmp << 4);
tmp = asc2nibble(buf[(2*i) + 11]);
if (tmp > 0x0F)
return;
isobuf[i] |= tmp;
}
ret = write(si, isobuf, items);
if(ret != items) {
PRINT_ERROR("Error in write()\n")
change_state(STATE_SHUTDOWN);
return;
}
break; /* COMMAND_SENDPDU */
case COMMAND_UNKNOWN:
default:
PRINT_ERROR("unknown command '%s'.\n", buf)
strcpy(buf, "< error unknown command >");
send(client_socket, buf, strlen(buf), 0);
break; /* COMMAND_UNKNOWN */
}
#endif
}
}
void state_isotp_init() {
int items, ret;
struct sockaddr_can addr;
struct ifreq ifr;
static struct can_isotp_options opts;
static struct can_isotp_fc_options fcopts;
char buf[MAXLEN]; /* inet commands to can */
while(previous_state != STATE_ISOTP) {
ret = receive_command(client_socket, buf);
if(ret != 0) {
change_state(STATE_SHUTDOWN);
return;
}
strncpy(ifr.ifr_name, bus_name, IFNAMSIZ);
if ( (ret = state_changed(buf, state)) ) {
/* ensure proper handling in other states */
if(ret == CONTROL_SWITCH_STATE) previous_state = STATE_ISOTP;
strcpy(buf, "< ok >");
send(client_socket, buf, strlen(buf), 0);
return;
}
#if 0
if(!strcmp("< echo >", buf)) {
send(client_socket, buf, strlen(buf), 0);
continue;
}
memset(&opts, 0, sizeof(opts));
memset(&fcopts, 0, sizeof(fcopts));
memset(&addr, 0, sizeof(addr));
/* get configuration to open the socket */
if(!strncmp("< isotpconf ", buf, 12)) {
items = sscanf(buf, "< %*s %x %x %x "
"%hhu %hhx %hhu "
"%hhx %hhx %hhx %hhx >",
&addr.can_addr.tp.tx_id,
&addr.can_addr.tp.rx_id,
&opts.flags,
&fcopts.bs,
&fcopts.stmin,
&fcopts.wftmax,
&opts.txpad_content,
&opts.rxpad_content,
&opts.ext_address,
&opts.rx_ext_address);
/* < isotpconf XXXXXXXX ... > check for extended identifier */
if(element_length(buf, 2) == 8)
addr.can_addr.tp.tx_id |= CAN_EFF_FLAG;
if(element_length(buf, 3) == 8)
addr.can_addr.tp.rx_id |= CAN_EFF_FLAG;
if (((opts.flags & CAN_ISOTP_RX_EXT_ADDR) && items < 10) ||
((opts.flags & CAN_ISOTP_EXTEND_ADDR) && items < 9) ||
((opts.flags & CAN_ISOTP_RX_PADDING) && items < 8) ||
((opts.flags & CAN_ISOTP_TX_PADDING) && items < 7) ||
(items < 5)) {
PRINT_ERROR("Syntax error in isotpconf command\n");
/* try it once more */
continue;
}
/* open ISOTP socket */
if ((si = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
PRINT_ERROR("Error while opening ISOTP socket %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
strcpy(ifr.ifr_name, bus_name);
if(ioctl(si, SIOCGIFINDEX, &ifr) < 0) {
PRINT_ERROR("Error while searching for bus %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
addr.can_family = PF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
/* only change the built-in defaults when required */
if (opts.flags)
setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
PRINT_VERBOSE("binding ISOTP socket...\n")
if (bind(si, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
PRINT_ERROR("Error while binding ISOTP socket %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
/* ok we made it and have a proper isotp socket open */
previous_state = STATE_ISOTP;
}
#else
ret = decode_command(buf);
switch(ret) {
case COMMAND_ECHO:
send(client_socket, buf, strlen(buf), 0);
continue;
break;
case COMMAND_ISOTPCONF:
memset(&opts, 0, sizeof(opts));
memset(&fcopts, 0, sizeof(fcopts));
memset(&addr, 0, sizeof(addr));
/* get configuration to open the socket */
items = sscanf(buf, "< %*s %x %x %x "
"%hhu %hhx %hhu "
"%hhx %hhx %hhx %hhx >",
&addr.can_addr.tp.tx_id,
&addr.can_addr.tp.rx_id,
&opts.flags,
&fcopts.bs,
&fcopts.stmin,
&fcopts.wftmax,
&opts.txpad_content,
&opts.rxpad_content,
&opts.ext_address,
&opts.rx_ext_address);
/* < isotpconf XXXXXXXX ... > check for extended identifier */
if(element_length(buf, 2) == 8)
addr.can_addr.tp.tx_id |= CAN_EFF_FLAG;
if(element_length(buf, 3) == 8)
addr.can_addr.tp.rx_id |= CAN_EFF_FLAG;
if (((opts.flags & CAN_ISOTP_RX_EXT_ADDR) && items < 10) ||
((opts.flags & CAN_ISOTP_EXTEND_ADDR) && items < 9) ||
((opts.flags & CAN_ISOTP_RX_PADDING) && items < 8) ||
((opts.flags & CAN_ISOTP_TX_PADDING) && items < 7) ||
(items < 5)) {
PRINT_ERROR("Syntax error in isotpconf command\n");
/* try it once more */
continue;
}
/* open ISOTP socket */
if ((si = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
PRINT_ERROR("Error while opening ISOTP socket %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
strcpy(ifr.ifr_name, bus_name);
if(ioctl(si, SIOCGIFINDEX, &ifr) < 0) {
PRINT_ERROR("Error while searching for bus %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
addr.can_family = PF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
/* only change the built-in defaults when required */
if (opts.flags)
setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
setsockopt(si, SOL_CAN_ISOTP, CAN_ISOTP_RECV_FC, &fcopts, sizeof(fcopts));
PRINT_VERBOSE("binding ISOTP socket...\n")
if (bind(si, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
PRINT_ERROR("Error while binding ISOTP socket %s\n", strerror(errno));
/* ensure proper handling in other states */
previous_state = STATE_ISOTP;
state_isotp_deinit();
change_state(STATE_SHUTDOWN);
return;
}
/* ok we made it and have a proper isotp socket open */
previous_state = STATE_ISOTP;
break;
case COMMAND_UNKNOWN:
default:
break;
} /* switch */
#endif
} /* while */
