int simple_test() {
StackMachine *sm = sm_new();
int i;
v4sf f, x= {0.11,0.14,-0.24,-0.85}, y= {0.711,0.223,-0.546, -7.661}, z= {0.1, 0.2, -0.3, -0.4};
sm_add_constant(sm, 0.5);
sm_add_constant(sm, 1.0);
sm_add_constant(sm, 0.0);
sm_add_constant(sm, 2.0);
sm_set_global(sm, 0, x[0]);
sm_set_global(sm, 1, y[1]);
sm_set_global(sm, 2, z[2]);
sm_set_stackcur(sm, 10);
sm_add_opcodes(sm, test_codes, testcodelen);
f = sm_run(sm, test_codes, testcodelen);
//printf("RESULT: %f, should be: %f\n", f, MAX(MAX(x, y), z)-0.5);
/*printf("[");
for (i=0; i<sm->stackcur+10; i++) {
if (i > 0) {
printf(", ");
}
printf("%.2f", sm->stack[i]);
}
printf("]\n");*/
}
// enable LE, setup ADV data
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
uint8_t adv_data[] = { 02, 01, 05, 03, 02, 0xf0, 0xff };
sm_run();
switch (packet_type) {
case HCI_EVENT_PACKET:
switch (packet[0]) {
case BTSTACK_EVENT_STATE:
// bt stack activated, get started
if (packet[2] == HCI_STATE_WORKING) {
printf("Working!\n");
hci_send_cmd(&hci_le_set_advertising_data, sizeof(adv_data), adv_data);
}
break;
case DAEMON_EVENT_HCI_PACKET_SENT:
att_try_respond();
break;
case HCI_EVENT_LE_META:
switch (packet[2]) {
case HCI_SUBEVENT_LE_CONNECTION_COMPLETE:
sm_response_handle = READ_BT_16(packet, 4);
sm_m_addr_type = packet[7];
BD_ADDR_COPY(sm_m_address, &packet[8]);
// TODO use non-null TK if appropriate
sm_reset_tk();
// TODO support private addresses
sm_s_addr_type = 0;
BD_ADDR_COPY(sm_s_address, hci_local_bd_addr());
// request security
sm_send_security_request = 1;
// reset connection MTU
att_connection.mtu = 23;
break;
case HCI_SUBEVENT_LE_LONG_TERM_KEY_REQUEST:
sm_s1(sm_tk, sm_m_random, sm_m_random, sm_s_ltk);
hci_send_cmd(&hci_le_long_term_key_request_reply, READ_BT_16(packet, 3), sm_s_ltk);
break;
default:
break;
}
break;
case HCI_EVENT_ENCRYPTION_CHANGE:
// distribute keys as requested by initiator
// TODO: handle initiator case here
if (sm_key_distribution_set & SM_KEYDIST_ENC_KEY)
sm_send_encryption_information = 1;
sm_send_master_identification = 1;
if (sm_key_distribution_set & SM_KEYDIST_ID_KEY)
sm_send_identity_information = 1;
sm_send_identity_address_information = 1;
if (sm_key_distribution_set & SM_KEYDIST_SIGN)
sm_send_signing_identification = 1;
break;
case HCI_EVENT_DISCONNECTION_COMPLETE:
att_response_handle = 0;
att_response_size = 0;
// restart advertising
hci_send_cmd(&hci_le_set_advertise_enable, 1);
break;
case HCI_EVENT_COMMAND_COMPLETE:
if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_advertising_parameters)){
// only needed for BLE Peripheral
hci_send_cmd(&hci_le_set_advertising_data, sizeof(adv_data), adv_data);
break;
}
if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_advertising_data)){
// only needed for BLE Peripheral
hci_send_cmd(&hci_le_set_scan_response_data, 10, adv_data);
break;
}
if (COMMAND_COMPLETE_EVENT(packet, hci_le_set_scan_response_data)){
// only needed for BLE Peripheral
hci_send_cmd(&hci_le_set_advertise_enable, 1);
break;
}
if (COMMAND_COMPLETE_EVENT(packet, hci_le_rand)){
switch (sm_state_responding){
case SM_STATE_C1_W4_RANDOM_A:
memcpy(&sm_s_random[0], &packet[6], 8);
hci_send_cmd(&hci_le_rand);
sm_state_responding++;
break;
case SM_STATE_C1_W4_RANDOM_B:
memcpy(&sm_s_random[8], &packet[6], 8);
// calculate s_confirm
sm_c1(sm_tk, sm_s_random, sm_preq, sm_pres, sm_m_addr_type, sm_s_addr_type, sm_m_address, sm_s_address, sm_s_confirm);
// send data
sm_state_responding = SM_STATE_C1_SEND;
break;
default:
break;
}
break;
}
}
}
sm_run();
}
static void sm_packet_handler(uint8_t packet_type, uint16_t handle, uint8_t *packet, uint16_t size){
if (packet_type != SM_DATA_PACKET) return;
printf("sm_packet_handler, request %0x\n", packet[0]);
switch (packet[0]){
case SM_CODE_PAIRING_REQUEST:
// store key distribtion request
sm_key_distribution_set = packet[6];
// for validate
memcpy(sm_preq, packet, 7);
// TODO use provided IO capabilites
// TOOD use local MITM flag
// TODO provide callback to request OOB data
memcpy(sm_response_buffer, packet, size);
sm_response_buffer[0] = SM_CODE_PAIRING_RESPONSE;
// sm_response_buffer[1] = 0x00; // io capability: DisplayOnly
// sm_response_buffer[1] = 0x02; // io capability: KeyboardOnly
// sm_response_buffer[1] = 0x03; // io capability: NoInputNoOutput
sm_response_buffer[1] = 0x04; // io capability: KeyboardDisplay
sm_response_buffer[2] = 0x00; // no oob data available
sm_response_buffer[3] = sm_response_buffer[3] & 3; // remove MITM flag
sm_response_buffer[4] = 0x10; // maxium encryption key size
sm_response_size = 7;
// for validate
memcpy(sm_pres, sm_response_buffer, 7);
break;
case SM_CODE_PAIRING_CONFIRM:
// received confirm value
memcpy(sm_m_confirm, &packet[1], 16);
// dummy
sm_response_size = 17;
memcpy(sm_response_buffer, packet, size);
// for real
// sm_state_responding = SM_STATE_C1_GET_RANDOM_A;
break;
case SM_CODE_PAIRING_RANDOM:
// received confirm value
memcpy(sm_m_random, &packet[1], 16);
sm_response_size = 17;
// validate
sm_validate();
//
memcpy(sm_response_buffer, packet, size);
break;
case SM_CODE_ENCRYPTION_INFORMATION:
sm_received_encryption_information = 1;
memcpy(sm_m_ltk, &packet[1], 16);
break;
case SM_CODE_MASTER_IDENTIFICATION:
sm_received_master_identification = 1;
sm_m_ediv = READ_BT_16(packet, 1);
memcpy(sm_m_rand, &packet[3],8);
break;
case SM_CODE_IDENTITY_INFORMATION:
sm_received_identity_information = 1;
memcpy(sm_m_irk, &packet[1], 16);
break;
case SM_CODE_IDENTITY_ADDRESS_INFORMATION:
sm_received_identity_address_information = 1;
sm_m_addr_type = packet[1];
BD_ADDR_COPY(sm_m_address, &packet[2]);
break;
case SM_CODE_SIGNING_INFORMATION:
sm_received_signing_identification = 1;
memcpy(sm_m_csrk, &packet[1], 16);
break;
}
// try to send preparared packet
sm_run();
}