PRIVATE u8_t syscall_notify(struct thread* th_from, struct proc* proc_to)
{
struct thread* th;
th = syscall_find_blocked_sender(th_from->proc,proc_to);
if (th != NULL)
{
arch_printf("%u unblocks %u via notify\n",th_from->proc->pid,proc_to->pid);
/* Set recipient ready for scheduling */
th->state = THREAD_READY;
/* End of sending */
th->ipc.state &= ~SYSCALL_IPC_SENDING;
/* Scheduler queues manipulation */
sched_dequeue(SCHED_BLOCKED_QUEUE,th);
sched_enqueue(SCHED_READY_QUEUE,th);
return IPC_SUCCESS;
}
//arch_printf("Notify failed\n");
//return IPC_FAILURE;
return IPC_SUCCESS;
}
/*
* Name : app_disconnect_func - Configure app at disconnection
*
* Scope : PUBLIC
*
* Arguments : task_id - id of the kernel task calling this function
* param - parameters passed from the stack
*
* Description : Configures keyboard application when connection is terminated.
*
* Returns : void
*
*/
void app_disconnect_func(ke_task_id_t const task_id, struct gapc_disconnect_ind const *param)
{
arch_printf("** Clear param update timer\r\n");
ke_timer_clear(APP_HID_TIMER, task_id);
// Call test code here
stop_kbd_single_test();
}
/*
* Name : app_param_update_func - Request update of connection params
*
* Scope : PUBLIC
*
* Arguments : task_id - id of the kernel task calling this function
* param - parameters passed from the stack
*
* Description : After connection and, optionally, pairing is completed, this function
* is called to (optionally) modify the connection parameters.
*
* Returns : void
*
*/
void app_param_update_func(void)
{
//Set a timer to update connection params after i.e. 10sec
ke_timer_set(APP_HID_TIMER, TASK_APP, 100);
arch_printf("** Set param update timer to 1 sec\r\n");
#ifndef MITM_REQUIRED
// Call test code here
#endif
}
/*
* Name : app_connection_func - Configure app at connection establishment
*
* Scope : PUBLIC
*
* Arguments : dest_id - id of the kernel task calling this function
* param - parameters passed from the stack
*
* Description : Configures keyboard application when connection is established.
*
* Returns : void
*
*/
void app_connection_func(ke_task_id_t const dest_id, struct gapc_connection_req_ind const *param)
{
/*--------------------------------------------------------------
* ENABLE REQUIRED PROFILES
*-------------------------------------------------------------*/
arch_printf("gap_le_create_conn_req_cmp_evt_handler() (%d, %d, %d, %d)\r\n",
(int)param->con_interval,
(int)param->con_latency,
(int)param->sup_to,
(int)param->clk_accuracy
);
app_env.conhdl = param->conhdl; // Store the connection handle
app_disc_enable_prf(param->conhdl);
app_basc_enable_prf(param->conhdl);
app_scppc_enable_prf(param->conhdl);
app_hogprh_enable_prf(param->conhdl);
}
/*
* Name : app_hid_timer_handler - Handler of the HID Timer
*
* Scope : PUBLIC
*
* Arguments : <various>
*
* Description : Sends a Connection Parameters Update Request to the Host
*
* Returns : KE_MSG_CONSUMED
*
*/
int app_hid_timer_handler(ke_msg_id_t const msgid,
void const *param,
ke_task_id_t const dest_id,
ke_task_id_t const src_id)
{
// arch_puts("app_hid_timer_handler()\r\n");
ke_state_t app_state = ke_state_get(TASK_APP);
// Modify Conn Params
if (app_state == APP_SECURITY || app_state == APP_PARAM_UPD || app_state == APP_CONNECTED)
{
struct gapc_param_update_cmd * req = KE_MSG_ALLOC(GAPC_PARAM_UPDATE_CMD, TASK_GAPC, TASK_APP, gapc_param_update_cmd);
req->operation = GAPC_UPDATE_PARAMS;
// Fill in the parameter structure
req->params.intv_min = 8; // N * 1.25ms
req->params.intv_max = 8; // N * 1.25ms
req->params.latency = 23; // Conn Events skipped
req->params.time_out = 200; // N * 10ms
arch_puts("Send GAP_PARAM_UPDATE_REQ\r\n");
ke_msg_send(req);
ke_state_set(TASK_APP, APP_PARAM_UPD);
}
// Keyboard test can start here!
uint32_t press = 100000; // 100 msec
uint32_t release = 235000; // 235 msec
uint32_t press_incr = 0; // 0 usec
uint32_t release_incr = 10; // 10 usec
uint32_t limit = 275000; // 275 msec
if ( !start_kbd_single_test(press, press_incr, release, release_incr, limit) ) {
arch_puts("### Failed to start test!\r\n");
} else {
arch_printf("### Started test (press: %d, incr: %d, release: %d, incr: %d, limit: %d)\r\n",
press, press_incr, release, release_incr, limit);
}
return (KE_MSG_CONSUMED);
}
PRIVATE u8_t syscall_receive(struct thread* th_receiver, struct proc* proc_sender)
{
struct thread* th_available = NULL;
/* Set receive state */
th_receiver->ipc.state |= SYSCALL_IPC_RECEIVING;
/* Set thread to receive from (can be NULL) */
th_receiver->ipc.recv_from = proc_sender;
/* Find a thread sending to me */
th_available = syscall_find_waiting_sender(th_receiver->proc, proc_sender);
/* A matching sender found ? */
if ( th_available != NULL )
{
/* Copy message from sender to receiver */
syscall_copymsg(th_available,th_receiver);
arch_printf("%u receives a message from %u\n",th_receiver->proc->pid,th_available->proc->pid);
/* Unblock sender */
th_available->state = THREAD_READY;
/* Set end of sending */
th_available->ipc.state &= ~SYSCALL_IPC_SENDING;
/* Remove sender from receiver waiting list et set it as ready for scheduling */
LLIST_REMOVE(th_receiver->proc->wait_list, th_available);
sched_enqueue(SCHED_READY_QUEUE, th_available);
arch_printf("%u unblock %u from its wait list\n",th_receiver->proc->pid,th_available->proc->pid);
/* End of reception */
th_receiver->ipc.state &= ~SYSCALL_IPC_RECEIVING;
}
else
{
/* No matching sender found: blocked waiting for a sender */
th_receiver->state = THREAD_BLOCKED;
/* Sched queues manipulation */
sched_dequeue(SCHED_READY_QUEUE, th_receiver);
sched_enqueue(SCHED_BLOCKED_QUEUE, th_receiver);
arch_printf("%u blocked cause no message available\n",th_receiver->proc->pid);
/* Current thread (receiver) is blocked, need scheduling */
struct thread* th;
th = sched_elect();
/* Change address space */
if (th->proc)
{
arch_switch_addrspace(th->proc->addrspace);
}
thread_switch_to(th);
}
return IPC_SUCCESS;
}
PRIVATE u8_t syscall_send(struct thread* th_sender, struct proc* proc_receiver)
{
struct thread* th_receiver;
/* There must be a receiver - No broadcast allow */
if ( proc_receiver == NULL )
{
arch_printf("send failed 1\n");
return IPC_FAILURE;
}
/* Check for deadlock */
if (syscall_deadlock(th_sender->proc,proc_receiver) == IPC_FAILURE)
{
arch_printf("deadlock\n");
return IPC_FAILURE;
}
/* No deadlock here, set state */
th_sender->ipc.state |= SYSCALL_IPC_SENDING;
/* Set destination */
th_sender->ipc.send_to = proc_receiver;
/* Get a thread willing to receive the message */
th_receiver = syscall_find_receiver(proc_receiver,th_sender->proc);
if (th_receiver != NULL)
{
/* Found a thread ! copy message from sender to receiver */
syscall_copymsg(th_sender,th_receiver);
arch_printf("%u sends a message to %u\n",th_sender->proc->pid,proc_receiver->pid);
/* Set end of reception */
th_receiver->ipc.state &= ~SYSCALL_IPC_RECEIVING;
/* Ready for scheduling */
th_receiver->state = THREAD_READY;
/* Scheduler queues manipulations */
sched_dequeue(SCHED_BLOCKED_QUEUE, th_receiver);
sched_enqueue(SCHED_READY_QUEUE, th_receiver);
arch_printf("%u unblock %u after send\n",th_sender->proc->pid,proc_receiver->pid);
/* Message is delivered to receiver, set end of sending */
th_sender->ipc.state &= ~SYSCALL_IPC_SENDING;
/* Scheduler queues manipulations (blocks sender) */
sched_dequeue(SCHED_READY_QUEUE, th_sender);
sched_enqueue(SCHED_BLOCKED_QUEUE, th_sender);
}
else
{
/* No receiving thread, enqueue in wait list */
sched_dequeue(SCHED_READY_QUEUE, th_sender);
LLIST_ADD(proc_receiver->wait_list,th_sender);
arch_printf("%u in wait list of %u\n",th_sender->proc->pid,proc_receiver->pid);
}
/* Sender is blocked, waiting for message processing (must be unblocked via notify) */
arch_printf("%u block after send\n",th_sender->proc->pid);
th_sender->state = THREAD_BLOCKED;
/* In any cases, current thread (sender) is blocked, so scheduling is needing */
struct thread* th;
th = sched_elect();
/* Change address space */
if (th->proc)
{
arch_switch_addrspace(th->proc->addrspace);
}
thread_switch_to(th);
return IPC_SUCCESS;
}
PUBLIC void syscall_handle(void)
{
struct proc* target_proc;
pid_t pid;
u32_t syscall_num;
u8_t res;
arch_printf("syscall_handle\n");
struct thread* th = cur_th;
/* Get current thread */
if (th == NULL)
{
res = IPC_FAILURE;
goto end;
}
/* Get syscall number from source register */
syscall_num = arch_ctx_get((arch_ctx_t*)th, ARCH_CONST_SOURCE);
/* Put originator proc into source register instead */
arch_ctx_set((arch_ctx_t*)th, ARCH_CONST_SOURCE,th->proc->pid);
/* Destination proc, stored in EDI */
pid = (pid_t)arch_ctx_get((arch_ctx_t*)th, ARCH_CONST_DEST);
if ( pid == IPC_ANY)
{
target_proc = NULL;
}
else
{
/* Get proc structure from given id */
target_proc = proc_pid(pid);
if ( target_proc == NULL )
{
res = IPC_FAILURE;
goto end;
}
}
/* Dispatch call to effective primitives */
switch(syscall_num)
{
case SYSCALL_SEND:
{
res = syscall_send(th, target_proc);
break;
}
case SYSCALL_RECEIVE:
{
res = syscall_receive(th, target_proc);
break;
}
case SYSCALL_NOTIFY:
{
res = syscall_notify(th, target_proc);
break;
}
default:
{
arch_printf("not a syscall number\n");
res = IPC_FAILURE;
break;
}
}
end:
/* Set result in caller's return register */
arch_ctx_set((arch_ctx_t*)th, ARCH_CONST_RETURN,res);
arch_printf("end of syscall :%u\n",arch_ctx_get((arch_ctx_t*)th, ARCH_CONST_RETURN));
return;
}
int hogprh_report_ind_handler(ke_msg_id_t const msgid,
struct hogprh_report_ind const *param,
ke_task_id_t const dest_id,
ke_task_id_t const src_id)
{
int i;
// arch_printf("HOGPRH: Report (conhdl=%d, hids_nb=%d, report_nb=%d, type=%d)\r\n",
// param->conhdl, param->hids_nb, param->report_nb, param->ind_type);
// arch_printf(" Data: 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\r\n",
// param->report[0],
// param->report[1],
// param->report[2],
// param->report[3],
// param->report[4],
// param->report[5],
// param->report[6],
// param->report[7]);
// Call test code here
// if all data are equal zero then it is a release report else a press
for (i = 0; i < 8; i++)
if (param->report[i] != 0)
break;
if (i < 8) { // press report
int ret;
ret = report_press();
if (ret)
{
arch_printf("Report (#%d): 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\r\n",
ret,
param->report[0],
param->report[1],
param->report[2],
param->report[3],
param->report[4],
param->report[5],
param->report[6],
param->report[7]);
arch_printf("Previous : 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\r\n",
prev_rep[0],
prev_rep[1],
prev_rep[2],
prev_rep[3],
prev_rep[4],
prev_rep[5],
prev_rep[6],
prev_rep[7]);
}
} else { // release report
int ret;
ret = report_release();
if (ret)
{
arch_printf("Report (#%d): 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\r\n",
ret,
param->report[0],
param->report[1],
param->report[2],
param->report[3],
param->report[4],
param->report[5],
param->report[6],
param->report[7]);
arch_printf("Previous : 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x\r\n",
prev_rep[0],
prev_rep[1],
prev_rep[2],
prev_rep[3],
prev_rep[4],
prev_rep[5],
prev_rep[6],
prev_rep[7]);
}
}
memcpy(prev_rep, param->report, 8);
return (KE_MSG_CONSUMED);
}
int hogprh_err_rsp_handler(ke_msg_id_t const msgid,
struct hogprh_char_req_rsp const *param,
ke_task_id_t const dest_id,
ke_task_id_t const src_id)
{
arch_printf("HOGPRH: Error for msgid=%d, conhdl=%d, att_code=%d\r\n", msgid, param->conhdl, param->att_code);
switch(param->status)
{
case PRF_ERR_INEXISTENT_HDL:
arch_puts("Handle not found!\r\n");
break;
case PRF_ERR_INVALID_PARAM:
arch_puts("Invalid param!\r\n");
break;
case PRF_ERR_NOT_WRITABLE:
arch_puts("Not writable!\r\n");
break;
default:
arch_printf("--> Status=%d\r\n", param->status);
if ( (param->status == ATT_ERR_INSUFF_AUTHEN) || (param->status == ATT_ERR_INSUFF_ENC) ){
#if MITM_REQUIRED
struct gapc_bond_cmd *req = KE_MSG_ALLOC(GAPC_BOND_CMD,
KE_BUILD_ID(TASK_GAPC, app_env.conidx), TASK_APP,
gapc_bond_cmd);
// Fill in the parameter structure
req->operation = GAPC_BOND;
// OOB information
req->pairing.oob = GAP_OOB_AUTH_DATA_NOT_PRESENT;
// Encryption key size
req->pairing.key_size = KEY_LEN;
// IO capabilities
req->pairing.iocap = GAP_IO_CAP_KB_DISPLAY;
// Authentication requirements
req->pairing.auth = GAP_AUTH_REQ_MITM_BOND;
//Security requirements
req->pairing.sec_req = GAP_SEC1_AUTH_PAIR_ENC;
//Initiator key distribution
req->pairing.ikey_dist = GAP_KDIST_SIGNKEY;
//Responder key distribution
req->pairing.rkey_dist = GAP_KDIST_ENCKEY;
// Send the message
ke_msg_send(req);
#else
struct gapc_bond_cmd *req = KE_MSG_ALLOC(GAPC_BOND_CMD,
KE_BUILD_ID(TASK_GAPC, app_env.conidx), TASK_APP,
gapc_bond_cmd);
// Fill in the parameter structure
req->operation = GAPC_BOND;
// OOB information
req->pairing.oob = GAP_OOB_AUTH_DATA_NOT_PRESENT;
// Encryption key size
req->pairing.key_size = KEY_LEN;
// IO capabilities
req->pairing.iocap = GAP_IO_CAP_KB_DISPLAY;
// Authentication requirements
req->pairing.auth = GAP_AUTH_REQ_NO_MITM_BOND;
//Security requirements
req->pairing.sec_req = GAP_NO_SEC;
//Initiator key distribution
req->pairing.ikey_dist = GAP_KDIST_SIGNKEY;
//Responder key distribution
req->pairing.rkey_dist = GAP_KDIST_ENCKEY;
// Send the message
ke_msg_send(req);
#endif
}
break;
}
return (KE_MSG_CONSUMED);
}
