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 */