int wpc_mgr_proctsfreq(char *buffer)
{
struct nsp_header * nsp_header = (struct nsp_header *) ((char *)buffer);
struct nsp_tsf_req * tsfrqst = (struct nsp_tsf_req *)(((char *)buffer) + NSP_HDR_LEN);
if(mrqst_state[mrqst_index].valid == 0) {
PRIN_LOG("%s: %d: Adding entry to request state: %d \n", __FUNCTION__, __LINE__, mrqst_index);
memcpy (&(mrqst_state[mrqst_index].tsfrqst), tsfrqst , sizeof(struct nsp_tsf_req));
mrqst_state[mrqst_index].type = NSP_TSFRESP;
gettimeofday(&(mrqst_state[mrqst_index].starttime), NULL);
mrqst_state[mrqst_index].timeout = tsfrqst->timeout * 1000;
mrqst_state[mrqst_index].valid = 1;
mrqst_state[mrqst_index].state = WPC_MGRSTATE_REQUESTINPROGRESS;
mrqst_index++;
if(mrqst_index >= WPC_MAX_CONCURRENTREQ)
mrqst_index = 0;
} else {
PRIN_LOG("%s: %d: Error: Insufficient buffering for requests %d \n", __FUNCTION__, __LINE__, mrqst_index);
}
return 1;
}
int wpc_mgr_proctsfresp(char *buffer)
{
struct nsp_mresp *mresp = (struct nsp_mresp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN);
int index = -1;
int count =0;
for(count =0; count < WPC_MAX_CONCURRENTREQ; count++ ) {
//Look up request
if(mresp->request_id == mrqst_state[count].mrqst.request_id && mrqst_state[count].valid == 1) {
index = count;
break;
}
}
if(index == -1) {
PRIN_LOG("%s: %d Could not find entry for response in state table.\n", __FUNCTION__, __LINE__);
return -1;
}
if((mrqst_state[index].type != NSP_TSFRESP ) || (mrqst_state[index].valid != 1)) {
PRIN_LOG("%s: %d: Error: Should be a Type 0 Resp and valid: %d %d \n", __FUNCTION__, __LINE__, mrqst_state[index].type, mrqst_state[index].valid );
return -1;
}
mrqst_state[index].valid = 0;
wpc_mgr_initstate(index);
printf("Recieved TSF Responses. Cancelling timeout \n");
return 1;
}
int wpc_mgr_calcstats(int index)
{
PRIN_LOG("%s: %d: Calculating stats for index: %d \n", __FUNCTION__, __LINE__, index);
//RTT and RSSI mean
int count, i;
double rtt_dev = 0.0, rssi_dev = 0.0, rssi_dev_t = 0.0;
wpc_probe_data *pd;
for (count =0; count < mrqst_state[index].proberespcount; count++) {
pd = &(mrqst_state[index].probedata[count]);
mrqst_state[index].rtt_mean+= pd->frtt;
for(i =0; i < MAX_CHAINS; i++)
mrqst_state[index].rssi_mean+=pd->rssi[i];
}
mrqst_state[index].rtt_mean= mrqst_state[index].rtt_mean/mrqst_state[index].proberespcount;
mrqst_state[index].rssi_mean= mrqst_state[index].rssi_mean/(mrqst_state[index].proberespcount *MAX_CHAINS);
PRIN_LOG("%s: %d: RTT Mean: %f RSSI mean %f\n", __FUNCTION__, __LINE__, mrqst_state[index].rtt_mean, mrqst_state[index].rssi_mean);
//RTT and RSSI Stddev
if(mrqst_state[index].proberespcount >1) {
for (count = 0; count < mrqst_state[index].proberespcount; count++) {
pd = &(mrqst_state[index].probedata[count]);
rtt_dev += pow(pd->frtt - mrqst_state[index].rtt_mean, 2);
for(i =0; i < MAX_CHAINS; i++)
rssi_dev_t +=pd->rssi[i];
rssi_dev_t = rssi_dev_t/MAX_CHAINS;
rssi_dev += pow(rssi_dev_t - mrqst_state[index].rssi_mean, 2);
}
mrqst_state[index].rtt_stddev = sqrt(rtt_dev/mrqst_state[index].proberespcount);
mrqst_state[index].rssi_stddev = sqrt(rssi_dev/mrqst_state[index].proberespcount);
PRIN_LOG("%s: %d: RTT STDDEV: %f RSSI STDDEV %f\n", __FUNCTION__, __LINE__, mrqst_state[index].rtt_stddev, mrqst_state[index].rssi_stddev);
}
//TBD
return 1;
}
int wpc_mgr_proctype1mresp(char *buffer)
{
struct nsp_mresp *mresp = (struct nsp_mresp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN);
int index = -1;
int count =0;
PRIN_LOG("Processing Type1 resp\n");
for(count =0; count < WPC_MAX_CONCURRENTREQ; count++ ) {
//Look up request
if(mresp->request_id == mrqst_state[count].mrqst.request_id && mrqst_state[count].valid == 1) {
index = count;
break;
}
}
if(index == -1) {
PRIN_LOG("%s: %d Could not find entry for response in state table.\n", __FUNCTION__, __LINE__);
return -1;
}
mrqst_state[index].proberespcount++;
if( mrqst_state[index].proberespcount == mrqst_state[index].mrqst.no_of_measurements ) {
mresp->result = 0;
mrqst_state[index].valid = 0;
PRIN_LOG("Recieved all responses. Cancelling timeout \n");
}
else
mresp->result = 1;
return 1;
}
/****************************************************
* Invoked from bluetooth stack via hdev->send()
* to send the packet out via ar6k to PAL firmware.
*
* For HCI command packet wmi_send_hci_cmd() is invoked.
* wmi_send_hci_cmd adds WMI_CMD_HDR and sends the packet
* to PAL firmware.
*
* For HCI ACL data packet wmi_data_hdr_add is invoked
* to add WMI_DATA_HDR to the packet. ar6000_acl_data_tx
* is then invoked to send the packet to PAL firmware.
******************************************************/
static int btpal_send_frame(struct sk_buff *skb)
{
struct hci_dev *hdev = (struct hci_dev *)skb->dev;
HCI_TRANSPORT_PACKET_TYPE type;
ar6k_hci_pal_info_t *pHciPalInfo;
A_STATUS status = A_OK;
struct sk_buff *txSkb = NULL;
AR_SOFTC_DEV_T *ar;
if (!hdev) {
PRIN_LOG("HCI PAL: btpal_send_frame - no device\n");
return -ENODEV;
}
if (!test_bit(HCI_RUNNING, &hdev->flags)) {
PRIN_LOG("HCI PAL: btpal_send_frame - not open\n");
return -EBUSY;
}
pHciPalInfo = (ar6k_hci_pal_info_t *)hdev->driver_data;
A_ASSERT(pHciPalInfo != NULL);
ar = pHciPalInfo->ar;
PRIN_LOG("+btpal_send_frame type: %d \n",bt_cb(skb)->pkt_type);
type = HCI_COMMAND_TYPE;
switch (bt_cb(skb)->pkt_type) {
case HCI_COMMAND_PKT:
type = HCI_COMMAND_TYPE;
hdev->stat.cmd_tx++;
break;
case HCI_ACLDATA_PKT:
type = HCI_ACL_TYPE;
hdev->stat.acl_tx++;
break;
case HCI_SCODATA_PKT:
/* we don't support SCO over the pal */
kfree_skb(skb);
return 0;
default:
A_ASSERT(FALSE);
kfree_skb(skb);
return 0;
}
if(loghci) {
A_PRINTF(">>> Send HCI %s packet len: %d\n",
(type == HCI_COMMAND_TYPE) ? "COMMAND" : "ACL",
skb->len);
if (type == HCI_COMMAND_TYPE) {
A_PRINTF("HCI Command: OGF:0x%X OCF:0x%X \r\n",
(HCI_GET_OP_CODE(skb->data)) >> 10,
(HCI_GET_OP_CODE(skb->data)) & 0x3FF);
}
DebugDumpBytes(skb->data,skb->len,"BT HCI SEND Packet Dump");
}
char * wpc_mgr_createtype0resp(int index)
{
struct nsp_header nsp_hdr;
struct nsp_type0_resp resp={0};
char * buf = malloc(NSP_HDR_LEN + TYPE0RES_LEN);
PRIN_LOG("%s: %d: Creating Type 0 response for index: %d \n", __FUNCTION__, __LINE__, index);
memset(buf, 0, sizeof(buf));
nsp_hdr.SFD = START_OF_FRAME;
nsp_hdr.version = NSP_VERSION;
nsp_hdr.frame_type = NSP_TYPE0RESP;
nsp_hdr.frame_length = TYPE0RES_LEN;
resp.request_id = mrqst_state[index].mrqst.request_id;
memcpy(resp.sta_mac_addr, mrqst_state[index].mrqst.sta_mac_addr, ETH_ALEN);
if ( mrqst_state[index].proberespcount > 0)
resp.result = 0;
else
resp.result = 1;
resp.rtt_mean = mrqst_state[index].rtt_mean;
resp.rtt_stddev = mrqst_state[index].rtt_stddev;
resp.rtt_samples = mrqst_state[index].proberespcount;
resp.rssi_mean = mrqst_state[index].rssi_mean;
resp.rssi_stddev = mrqst_state[index].rssi_stddev;
resp.rssi_samples = mrqst_state[index].proberespcount *MAX_CHAINS;
memcpy(buf, &nsp_hdr, NSP_HDR_LEN);
memcpy((buf + NSP_HDR_LEN), &resp, TYPE0RES_LEN);
return buf;
}
int wpc_mgr_proctype0mreq(char *buffer)
{
//Check if state entry exists for this STA
struct nsp_header * nsp_header = (struct nsp_header *) ((char *)buffer);
struct nsp_mrqst * mrqst = (struct nsp_mrqst *)(((char *)buffer) + NSP_HDR_LEN);
PRIN_LOG("Processing Type0 resp\n");
if(mrqst_state[mrqst_index].valid == 0) {
PRIN_LOG("%s: %d: Adding entry to request state: %d \n", __FUNCTION__, __LINE__, mrqst_index);
//Setup state
memcpy (&(mrqst_state[mrqst_index].mrqst), mrqst , sizeof(struct nsp_mrqst));
mrqst_state[mrqst_index].type = NSP_TYPE0RESP;
gettimeofday(&(mrqst_state[mrqst_index].starttime), NULL);
mrqst_state[mrqst_index].timeout = mrqst->timeout * 1000;
mrqst_state[mrqst_index].valid = 1;
mrqst_state[mrqst_index].state = WPC_MGRSTATE_REQUESTINPROGRESS;
mrqst_state[mrqst_index].proberespcount = 0;
/*
if((mrqst->mode & RX_CHAINMASK_HASH) == RX_CHAINMASK_1)
mrqst_state[mrqst_index].pkt_corr_info.nrx_chains = 1;
if((mrqst->mode & RX_CHAINMASK_HASH) == RX_CHAINMASK_2)
mrqst_state[mrqst_index].pkt_corr_info.nrx_chains = 2;
if((mrqst->mode & RX_CHAINMASK_HASH) == RX_CHAINMASK_3)
mrqst_state[mrqst_index].pkt_corr_info.nrx_chains = 3;
*/
mrqst_index++;
if(mrqst_index >= WPC_MAX_CONCURRENTREQ)
mrqst_index = 0;
} else {
PRIN_LOG("%s: %d: Error: Insufficient buffering for requests %d \n", __FUNCTION__, __LINE__, mrqst_index);
}
//Pass Measurement Request to the driver: The request is changed from Type 0 to Type 1
mrqst->mode &= NSP_MRQSTTYPE_TYPE1;
PRIN_LOG("%s: %d: Received a Type 0 Measurement Request from Server\n", __FUNCTION__, __LINE__);
PRIN_LOG("%s: %d: Will send Type 1 Request to Driver\n", __FUNCTION__, __LINE__);
return 0;
}
/***************************************
* bt_open - open a handle to the device
***************************************/
static int bt_open(struct hci_dev *hdev)
{
PRIN_LOG("HCI PAL: bt_open - enter - x\n");
set_bit(HCI_RUNNING, &hdev->flags);
set_bit(HCI_UP, &hdev->flags);
set_bit(HCI_INIT, &hdev->flags);
return 0;
}
int wpc_mgr_sendtype0resp(int index)
{
int recv_len, written;
int count;
char * buffer = wpc_mgr_createtype0resp(index);
//Send Type0 message to Server
if(mrqst_state[index].proberespcount > 0) {
PRIN_LOG("%s: %d: Sending Type 0 response for index: %d Result : Success\n", __FUNCTION__, __LINE__, index);
recv_len = NSP_HDR_LEN + TYPE0RES_LEN;
written = wpc_write(wpc_server_sock, ((char *)buffer), recv_len);
if ( written < recv_len) {
// TODO : Need to write pending date if there is partial write
printf("Partial write Closing connection!!!!! Size %d written %d\n",
sizeof(struct nsp_header)+sizeof(struct nsp_mresp), written);
if(wpc_server_sock > 0) {
FD_CLR(wpc_server_sock, &read_fds);
wpc_close(wpc_server_sock);
wpc_server_sock = -1;
}
}
//Delete all other responses (Free memory)
for (count =0; count < mrqst_state[index].proberespcount; count++) {
//TBD
}
} else {
PRIN_LOG("%s: %d: Sending Type 0 response for index: %d Result : Fail\n", __FUNCTION__, __LINE__, index);
//If no responses, result = fail
//TBD
}
//TBD
free(buffer);
return 1;
}
int wpc_mgr_handletimeout()
{
int count;
struct timeval tv;
int elapsed;
int timers = 0;
gettimeofday(&tv, NULL);
//Iterate through pending requests. Check if any of them have timed out.
for(count =0; count < WPC_MAX_CONCURRENTREQ; count++ ) {
if(mrqst_state[count].valid == 1) {
timers++;
elapsed = (tv.tv_sec - mrqst_state[count].starttime.tv_sec) * 1000;
elapsed += (tv.tv_usec - mrqst_state[count].starttime.tv_usec) / 1000;
PRIN_LOG("%s: %d: Checking index: %d Elapsed time: %d\n", __FUNCTION__, __LINE__, count, elapsed);
PRIN_LOG("%s: %d: Checking index: %d Starttime : %d %d\n", __FUNCTION__, __LINE__, count, mrqst_state[count].starttime.tv_sec, mrqst_state[count].starttime.tv_usec);
PRIN_LOG("%s: %d: Checking index: %d Current time : %d %d\n", __FUNCTION__, __LINE__, count, tv.tv_sec, tv.tv_usec);
if(elapsed > mrqst_state[count].timeout/1000) {
PRIN_LOG("%s: %d: Request timed out. Will send Type 0 Response to Server\n", __FUNCTION__, __LINE__);
//Send response
if (mrqst_state[count].type == NSP_TYPE0RESP)
{
//Calculate statistics
PRIN_LOG("%s: %d: Request timed out. Will send Type 0 Response to Server\n", __FUNCTION__, __LINE__);
wpc_mgr_calcstats(count);
wpc_mgr_sendtype0resp(count);
mrqst_state[count].valid = 0;
}
else if (mrqst_state[count].proberespcount < mrqst_state[count].mrqst.no_of_measurements && mrqst_state[count].type == NSP_TYPE1RESP)
{
send_empty_mresponse(mrqst_state[count].mrqst.request_id, REQUEST_TIMEOUT);
mrqst_state[count].valid = 0;
PRIN_LOG("Sending Empty Mresponse\n");
}
else if (mrqst_state[count].type == NSP_TSFRESP)
{
send_empty_tsfresponse(mrqst_state[count].mrqst.request_id, REQUEST_TIMEOUT);
mrqst_state[count].valid = 0;
PRIN_LOG("Sending Empty TSF Reponse\n");
}
else {
PRIN_LOG("Doesn't meet any condition mrqst_state[mrqst_index].type %d\n",mrqst_state[count].type);
}
//Delete state entry
wpc_mgr_initstate(count);
}
}
}
}
//Check to see if server expect a response of Type 0 or Type 1
int wpc_mgr_checkresptype(char *buffer)
{
int index = -1;
int count =0;
struct nsp_header * nsp_header = (struct nsp_header *) ((char *)buffer + sizeof(struct nlmsghdr));
struct nsp_mresp *mresp = (struct nsp_mresp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN);
struct nsp_type1_resp *resp = (struct nsp_type1_resp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN + MRES_LEN);
//Check to see what type of resposne shoudl be sent to the server.
//The WPC <-> Driver exchange only Type 1 messages. However the Server
//may have requested a Type 0 response. If so, the WPC needs to process and convert the message.
for(count =0; count < WPC_MAX_CONCURRENTREQ; count++ ) {
//Look up request
if((mresp->request_id == mrqst_state[count].mrqst.request_id) && mrqst_state[count].valid == 1) {
index = count;
break;
}
}
PRIN_LOG("%s: %d: Found state index: %d\n", __FUNCTION__, __LINE__, index);
if (index == -1) {
//Request not logged in state array. WPC does not log Type1 requests.
PRIN_LOG("%s: %d: Could find any index to locate this response\n", __FUNCTION__, __LINE__);
return -1;
}
if(mrqst_state[index].type == NSP_TYPE1RESP ) {
//Server has requested a Type 1 message. Forward this response to the Server
PRIN_LOG("%s: %d: Received a Type 1 Measurement Response\n", __FUNCTION__, __LINE__);
PRIN_LOG("%s: %d: Will send Type 1 Response to Server\n", __FUNCTION__, __LINE__);
return 1;
} else if(mrqst_state[index].type == NSP_TYPE0RESP ) {
//Server has requested a Type 0 message. Perform processing and Forward this response to the Server
PRIN_LOG("%s: %d: Received a Type 1 Measurement Response\n", __FUNCTION__, __LINE__);
PRIN_LOG("%s: %d: Will send Type 0 Response to Server\n", __FUNCTION__, __LINE__);
return 0;
}
}
int wpc_mgr_procdrivermsg()
{
int recvlen;
int written;
char buffer[MAX_PAYLOAD];
int type;
// handle data sent up by kernel on the netlink socket
recvlen = wpc_recv(wpc_driver_sock, buffer, MAX_PAYLOAD, 0);
if( recvlen <= 0 ) {
wpc_close(wpc_driver_sock);
if(wpc_driver_sock > 0)
FD_CLR(wpc_driver_sock, &read_fds);
}
else {
struct nsp_mresp *res;
struct nsp_sresp *sres;
struct nsp_header *nsp_header;
struct nsp_type1_resp *type1_resp;
struct nsp_cap_resp *cap_resp;
struct nsp_station_info *sta_info;
struct timeval tv;
time_t curtime;
char time_buf[30];
unsigned int recv_len;
int i;
int send = 1;
if (wpc_server_sock > 0) {
nsp_header = (struct nsp_header *) ((char *)buffer + sizeof(struct nlmsghdr));
print_nsp_header(nsp_header);
switch( nsp_header->frame_type ) {
//TBD: read length from frame header
case NSP_WAKEUPRESP:
case NSP_STARETURNTOSLEEPRES:
recv_len = NSP_HDR_LEN + WAKEUPRES_LEN;
break;
case NSP_TSFRESP:
recv_len = NSP_HDR_LEN + TSFRES_LEN;
wpc_mgr_proctsfresp(buffer);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_tsfresp((struct nsp_tsf_resp *)(((char *)buffer)+sizeof(struct nlmsghdr)
+ NSP_HDR_LEN));
}
break;
case NSP_CRESP:
recv_len = NSP_HDR_LEN + CAPRES_LEN;
cap_resp = (struct nsp_cap_resp *)((char *)buffer + sizeof(struct nlmsghdr)
+ sizeof(struct nsp_header));
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_cap_resp(cap_resp);
}
break;
case NSP_SRESP:
sres = (struct nsp_sresp *)((char *)buffer + sizeof(struct nlmsghdr)
+ sizeof(struct nsp_header));
recv_len = NSP_HDR_LEN + SRES_LEN + sres->no_of_vaps * VAPINFO_LEN + sres->no_of_stations*SINFO_LEN;
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
//print_nsp_header(nsp_header);
print_sresp(sres);
}
break;
case NSP_TYPE1RESP:
res = (struct nsp_mresp *)((char *)buffer + sizeof(struct nlmsghdr)
+ sizeof(struct nsp_header));
gettimeofday(&tv, NULL);
curtime=tv.tv_sec;
strftime(time_buf,30,"%m-%d-%Y %T.",localtime(&curtime));
PRIN_LOG("Resp Time%s %ld\n",time_buf,tv.tv_usec);
type1_resp = (struct nsp_type1_resp *)((char *)buffer
+ sizeof(struct nlmsghdr) + sizeof(struct nsp_header) + MRES_LEN);
recv_len = NSP_HDR_LEN + MRES_LEN + (res->no_of_responses * TYPE1RES_LEN);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
//Response from the driver will be Type 1,
//check what response has been requested by server
type = wpc_mgr_checkresptype(buffer);
if (type==1)
send = wpc_mgr_proctype1mresp(buffer);
else if(type==0)
send = wpc_mgr_proctype0mresp(buffer, recv_len);
else {
printf("Drop packet as typeis %d\n",type);
send = -1;
}
// print_nsp_header(nsp_header);
print_mresp(res);
print_type1_resp(type1_resp);
}
break;
case NSP_TYPE0RESP:
printf("Driver can't send Type0 responses\n");
wpc_mgr_proctype0mresp(buffer, 0);
break;
default:
printf("Unknown frame type\n");
break;
}
if ( send >= 0 ) {
written = wpc_write(wpc_server_sock, ((char *)buffer)+sizeof(struct nlmsghdr), recv_len);
if ( written < recv_len) {
// TBD : Need to write pending data if there is partial write
printf("Partial write closing connection Size: %d Written: %d\n", sizeof(struct nsp_header)+sizeof(struct nsp_mresp), written);
if(wpc_server_sock > 0)
{
wpc_close(wpc_server_sock);
FD_CLR(wpc_server_sock, &read_fds);
wpc_server_sock = -1;
}
}
}
else {
printf("Dropping frame as send = %d\n", send);
}
}
}
}
/***************************************
* bt_close - close handle to the device
***************************************/
static int bt_close(struct hci_dev *hdev)
{
PRIN_LOG("HCI PAL: bt_close - enter\n");
clear_bit(HCI_RUNNING, &hdev->flags);
return 0;
}
/*****************************
* bt_ioctl - ioctl processing
*****************************/
static int bt_ioctl(struct hci_dev *hdev, unsigned int cmd, unsigned long arg)
{
PRIN_LOG("HCI PAL: bt_ioctl - enter\n");
return -ENOIOCTLCMD;
}
/**************************************
* bt_flush - flush outstanding packets
**************************************/
static int bt_flush(struct hci_dev *hdev)
{
PRIN_LOG("HCI PAL: bt_flush - enter\n");
return 0;
}
int wpc_mgr_proctype0mresp(char * buffer, int recv_len)
{
int chainCnt = 0;
wpc_probe_data *pd;
struct nsp_header * nsp_header = (struct nsp_header *) ((char *)buffer + sizeof(struct nlmsghdr));
//The server request is for a message of Type 0. However, the request is always sent to the driver as Type 1.
//Hence extract the Type 1 fields from the response.
struct nsp_mresp *mresp = (struct nsp_mresp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN);
struct nsp_type1_resp *resp = (struct nsp_type1_resp *)((char *)buffer + sizeof(struct nlmsghdr) + NSP_HDR_LEN + MRES_LEN);
int index = -1;
int count =0;
int probeindex =0;
u_int8_t rxchains =0;
int written = 0;
for(count =0; count < WPC_MAX_CONCURRENTREQ; count++ ) {
//Look up request
if(mresp->request_id == mrqst_state[count].mrqst.request_id && mrqst_state[count].valid == 1) {
index = count;
break;
}
}
if(index == -1) {
PRIN_LOG("%s: %d Could not find entry for response in state table.\n", __FUNCTION__, __LINE__);
return -1;
}
if((mrqst_state[index].type != NSP_TYPE0RESP ) || (mrqst_state[index].valid != 1)) {
PRIN_LOG("%s: %d: Error: Should be a Type 0 Resp and valid: %d %d \n", __FUNCTION__, __LINE__, mrqst_state[index].type, mrqst_state[index].valid );
return -1;
}
//Store data
probeindex = mrqst_state[index].proberespcount;
pd = &(mrqst_state[index].probedata[probeindex]);
mrqst_state[index].proberespcount++;
pd->tod = resp->tod;
pd->toa = resp->toa;
/* We keep track of RTT from all the chains, and in the end we use it for outliers */
for (chainCnt = 0; chainCnt < MAX_CHAINS; chainCnt++)
pd->rtt[chainCnt] = resp->toa - resp->tod;
pd->txrate = resp->send_rate;
memcpy(pd->rssi, resp->rssi, sizeof(u_int8_t)*MAX_CHAINS);
memcpy(pd->cd, resp->type1_payld, sizeof(u_int8_t)*TYPE1PAYLDLEN);
// rxchains = (u_int8_t) ((mrqst_state[index].mrqst.mode & RX_CHAINMASK_3) >> 7);
rxchains = resp->no_of_chains;
PRIN_LOG("%s: %d: Received a measurement response from driver. RTT: %d RSSI: %d %d %d\n", __FUNCTION__, __LINE__, pd->frtt, pd->rssi[0], pd->rssi[1], pd->rssi[2]);
PRIN_LOG("%s rxchains: %d, mrqst.ode: %x \n", __func__, rxchains, mrqst_state[index].mrqst.mode);
//Calculate Type 0 correction
wpc_rtte_calculate_ftoa(pd, rxchains);
printf("mrqst_state[index].mrqst.no_of_measurements %d == mrqst_state[index].proberespcount %d\n", mrqst_state[index].mrqst.no_of_measurements , mrqst_state[index].proberespcount);
//If all probes have been received, then send response
if(mrqst_state[index].mrqst.no_of_measurements == mrqst_state[index].proberespcount) {
PRIN_LOG("%s: %d: Received all measurement responses from driver. Sending Measurement Response to server Index: %d\n", __FUNCTION__, __LINE__, index);
//Calculate statistics
wpc_mgr_calcstats(index);
if ( recv_len ) {
written = wpc_write(wpc_server_sock, ((char *)buffer)+sizeof(struct nlmsghdr), recv_len);
if ( written < recv_len) {
// TBD : Need to write pending data if there is partial write
printf("Partial write closing connection Size: %d Written: %d\n", sizeof(struct nsp_header)+sizeof(struct nsp_mresp), written);
if(wpc_server_sock > 0)
{
wpc_close(wpc_server_sock);
FD_CLR(wpc_server_sock, &read_fds);
wpc_server_sock = -1;
}
}
}
//Send response
wpc_mgr_sendtype0resp(index);
//Delete state entry
wpc_mgr_initstate(index);
return -1;
} else {
PRIN_LOG("%s: %d: Waiting for more measurement responses from driver \n", __FUNCTION__, __LINE__);
//Extend the timeout period
gettimeofday(&(mrqst_state[index].starttime), NULL);
}
return 1;
}
/***************
* bt_destruct
***************/
static void bt_destruct(struct hci_dev *hdev)
{
PRIN_LOG("HCI PAL: bt_destruct - enter\n");
/* nothing to do here */
}
int wpc_mgr_procservermsg()
{
int nbytes;
int written;
int write_len;
int fd;
u_int8_t buf[MAX_PAYLOAD]={'\0'};
nbytes = wpc_recv(wpc_server_sock, buf, NSP_HDR_LEN, 0);
struct nsp_header *nsp_header = (struct nsp_header *) buf;
struct nsp_header nsp_hdr;
int len = 0;
//TBD: read length from frame header
switch (nsp_header->frame_type)
{
case NSP_MRQST:
len = MREQ_LEN;
break;
case NSP_SRQST:
len = SREQ_LEN;
break;
case NSP_CRQST:
len = CAPREQ_LEN;
break;
case NSP_WAKEUPRQST:
len = WAKEUPREQ_LEN;
break;
case NSP_TSFRQST:
len = TSFREQ_LEN;
break;
case NSP_STARETURNTOSLEEPREQ:
len = SLEEPREQ_LEN;
break;
case NSP_WPCDBGRGST:
len = WPCDBGREQ_LEN;
break;
}
print_nsp_header(nsp_header);
nbytes += wpc_recv(wpc_server_sock, buf+NSP_HDR_LEN, len, 0);
if (nbytes <= 0) {
//error or connection closed by client
if (nbytes == 0)
printf("selectserver: socket %d hung up\n", fdcount);
else
printf("recv failed\n");
wpc_close(wpc_server_sock);
if(wpc_server_sock > 0)
FD_CLR(wpc_server_sock, &read_fds);
wpc_server_sock = -1;
} else {
buf[nbytes] = '\0';
struct nsp_mrqst *mrqst;
struct nsp_sreq *srqst;
struct nsp_cap_req *crqst;
struct nsp_wakeup_req *wrqst;
struct nsp_tsf_req *tsfrqst;
struct nsp_sleep_req *slrqst;
struct nsp_wpc_dbg_req *wpcdbgrqst;
struct nsp_wpc_dbg_resp wpcdbgresp;
struct request_no *reqno;
struct timeval tv;
time_t req_curtime;
char time_buf[30];
reqno = (struct request_no *)(((char *)buf) + NSP_HDR_LEN);
if( nsp_header->version > NSP_VERSION ) {
printf("ERROR: Request protocol version no (%02x) > Current Version (%02x)\n",nsp_header->version, NSP_VERSION);
send_error_msg(reqno->request_id, NSP_UNSUPPORTED_PROTOCOL_VERSION);
return -1;
}
switch (nsp_header->frame_type)
{
case NSP_MRQST:
mrqst = (struct nsp_mrqst *)(((char *)buf) + NSP_HDR_LEN);
request_id = mrqst->request_id;
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_mrqst(mrqst);
gettimeofday(&tv, NULL);
req_curtime=tv.tv_sec;
strftime(time_buf,30,"%m-%d-%Y %T.",localtime(&req_curtime));
PRIN_LOG("Req Time%s %ld\n",time_buf,tv.tv_usec);
if((mrqst->mode & NSP_MRQSTTYPE_MASK) == NSP_MRQSTTYPE_TYPE0) {
PRIN_LOG("%s: %d: Received a Type 0 Measurement Request\n", __FUNCTION__, __LINE__);
wpc_mgr_proctype0mreq(buf);
}else if((mrqst->mode & NSP_MRQSTTYPE_MASK) == NSP_MRQSTTYPE_TYPE1) {
PRIN_LOG("%s: %d: Received a Type 1 Measurement Request\n", __FUNCTION__, __LINE__);
wpc_mgr_proctype1mreq(buf);
}
}
break;
case NSP_SRQST:
srqst = (struct nsp_sreq *)(((char *)buf) + NSP_HDR_LEN);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_srqst(srqst);
}
break;
case NSP_CRQST:
crqst = (struct nsp_cap_req *)(((char *)buf) + NSP_HDR_LEN);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_crqst(crqst);
}
break;
case NSP_WAKEUPRQST:
wrqst = (struct nsp_wakeup_req *) (((char *)buf) + NSP_HDR_LEN);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_wrqst(wrqst);
}
break;
case NSP_TSFRQST:
tsfrqst = (struct nsp_tsf_req *) (((char *)buf) + NSP_HDR_LEN);
wpc_mgr_proctsfreq(buf);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
print_tsfrqst(tsfrqst);
break;
case NSP_STARETURNTOSLEEPREQ:
slrqst = (struct nsp_sleep_req *) (((char *)buf) + NSP_HDR_LEN);
if (wpc_debug) {
if (wpc_dbg_log_len + WPCDBGHDR_LEN <= WPC_DBG_SIZE) {
dump_wpc_log(__LINE__, nsp_header->frame_type);
}
else {
printf("wpc log file is full:cannot written\n");
return;
}
}
else {
print_slrqst(slrqst);
}
break;
}
if(nsp_header->frame_type == NSP_WPCDBGRGST) {
wpcdbgrqst = (struct nsp_wpc_dbg_resp *) (((char *)buf) + NSP_HDR_LEN);
print_wpcdbgrqst(wpcdbgrqst);
if (wpcdbgrqst->dbg_mode) {
if( !wpc_debug ) {
wpc_debug = 1;
fd = write_dbg_log(fd);
}
}
else {
if (wpc_debug) {
wpc_debug = 0;
stop_dbg_log(fd);
}
}
nsp_hdr.SFD = START_OF_FRAME;
nsp_hdr.version = NSP_VERSION;
nsp_hdr.frame_type = NSP_WPCDBGRESP;
nsp_hdr.frame_length = WPCDBGRESP_LEN;
memcpy(buf, &nsp_hdr, NSP_HDR_LEN);
print_nsp_header(&nsp_hdr);
wpcdbgresp.request_id = wpcdbgrqst->request_id;
wpcdbgresp.result = 0;
memcpy((buf + NSP_HDR_LEN), &wpcdbgresp, WPCDBGRESP_LEN);
print_wpcdbgresp(&wpcdbgresp);
write_len = NSP_HDR_LEN + WPCDBGRESP_LEN;
written = wpc_write(wpc_server_sock, ((char *)buf), write_len);
if ( written < write_len) {
// TBD : Need to write pending data if there is partial write
printf("Partial write closing connection Size: %d Written: %d\n", sizeof(struct nsp_header)+sizeof(struct nsp_mresp), written);
wpc_close(wpc_server_sock);
FD_CLR(wpc_server_sock, &read_fds);
wpc_server_sock = -1;
}
}
else {
wpc_nlsendmsg(wpc_driver_sock,nbytes,buf,nlh,&iov,&msg);
}
}
}
