Home | History | Annotate | Download | only in sap
      1 /*
      2  *  BlueZ - Bluetooth protocol stack for Linux
      3  *
      4  *  Copyright (C) 2010 Instituto Nokia de Tecnologia - INdT
      5  *  Copyright (C) 2010 ST-Ericsson SA
      6  *  Copyright (C) 2011 Tieto Poland
      7  *
      8  *  Author: Marek Skowron <marek.skowron (at) tieto.com> for ST-Ericsson.
      9  *  Author: Waldemar Rymarkiewicz <waldemar.rymarkiewicz (at) tieto.com>
     10  *          for ST-Ericsson.
     11  *
     12  *  This program is free software; you can redistribute it and/or modify
     13  *  it under the terms of the GNU General Public License as published by
     14  *  the Free Software Foundation; either version 2 of the License, or
     15  *  (at your option) any later version.
     16  *
     17  *  This program is distributed in the hope that it will be useful,
     18  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
     19  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     20  *  GNU General Public License for more details.
     21  *
     22  *  You should have received a copy of the GNU General Public License
     23  *  along with this program; if not, write to the Free Software
     24  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
     25  */
     26 
     27 #ifdef HAVE_CONFIG_H
     28 #include <config.h>
     29 #endif
     30 
     31 #include <stdio.h>
     32 #include <errno.h>
     33 #include <glib.h>
     34 #include <netinet/in.h>
     35 #include <bluetooth/bluetooth.h>
     36 #include <bluetooth/sdp.h>
     37 #include <bluetooth/sdp_lib.h>
     38 
     39 #include "adapter.h"
     40 #include "btio.h"
     41 #include "sdpd.h"
     42 #include "log.h"
     43 #include "error.h"
     44 #include "dbus-common.h"
     45 #include "sap.h"
     46 #include "server.h"
     47 
     48 #define SAP_SERVER_INTERFACE	"org.bluez.SimAccess"
     49 #define SAP_UUID		"0000112D-0000-1000-8000-00805F9B34FB"
     50 #define SAP_SERVER_CHANNEL	8
     51 #define SAP_BUF_SIZE		512
     52 
     53 #define PADDING4(x) (4 - (x & 0x03))
     54 #define PARAMETER_SIZE(x) (sizeof(struct sap_parameter) + x + PADDING4(x))
     55 
     56 #define SAP_NO_REQ 0xFF
     57 
     58 #define SAP_TIMER_GRACEFUL_DISCONNECT 30
     59 #define SAP_TIMER_NO_ACTIVITY 30
     60 
     61 enum {
     62 	SAP_STATE_DISCONNECTED,
     63 	SAP_STATE_CONNECT_IN_PROGRESS,
     64 	SAP_STATE_CONNECTED,
     65 	SAP_STATE_GRACEFUL_DISCONNECT,
     66 	SAP_STATE_IMMEDIATE_DISCONNECT,
     67 	SAP_STATE_CLIENT_DISCONNECT
     68 };
     69 
     70 struct sap_connection {
     71 	GIOChannel *io;
     72 	uint32_t state;
     73 	uint8_t processing_req;
     74 	guint timer_id;
     75 };
     76 
     77 struct sap_server {
     78 	bdaddr_t src;
     79 	char *path;
     80 	uint32_t record_id;
     81 	GIOChannel *listen_io;
     82 	struct sap_connection *conn;
     83 };
     84 
     85 static DBusConnection *connection;
     86 static struct sap_server *server;
     87 
     88 static void start_guard_timer(struct sap_connection *conn, guint interval);
     89 static void stop_guard_timer(struct sap_connection *conn);
     90 static gboolean guard_timeout(gpointer data);
     91 
     92 static size_t add_result_parameter(uint8_t result,
     93 					struct sap_parameter *param)
     94 {
     95 	param->id = SAP_PARAM_ID_RESULT_CODE;
     96 	param->len = htons(SAP_PARAM_ID_RESULT_CODE_LEN);
     97 	*param->val = result;
     98 
     99 	return PARAMETER_SIZE(SAP_PARAM_ID_RESULT_CODE_LEN);
    100 }
    101 
    102 static int is_power_sim_off_req_allowed(uint8_t processing_req)
    103 {
    104 	switch (processing_req) {
    105 	case SAP_NO_REQ:
    106 	case SAP_TRANSFER_APDU_REQ:
    107 	case SAP_TRANSFER_ATR_REQ:
    108 	case SAP_POWER_SIM_ON_REQ:
    109 	case SAP_RESET_SIM_REQ:
    110 	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
    111 		return 1;
    112 	default:
    113 		return 0;
    114 	}
    115 }
    116 
    117 static int is_reset_sim_req_allowed(uint8_t processing_req)
    118 {
    119 	switch (processing_req) {
    120 	case SAP_NO_REQ:
    121 	case SAP_TRANSFER_APDU_REQ:
    122 	case SAP_TRANSFER_ATR_REQ:
    123 	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
    124 		return 1;
    125 	default:
    126 		return 0;
    127 	}
    128 }
    129 
    130 static int check_msg(struct sap_message *msg)
    131 {
    132 	if (!msg)
    133 		return -EINVAL;
    134 
    135 	switch (msg->id) {
    136 	case SAP_CONNECT_REQ:
    137 		if (msg->nparam != 0x01)
    138 			return -EBADMSG;
    139 
    140 		if (msg->param->id != SAP_PARAM_ID_MAX_MSG_SIZE)
    141 			return -EBADMSG;
    142 
    143 		if (ntohs(msg->param->len) != SAP_PARAM_ID_MAX_MSG_SIZE_LEN)
    144 			return -EBADMSG;
    145 
    146 		break;
    147 
    148 	case SAP_TRANSFER_APDU_REQ:
    149 		if (msg->nparam != 0x01)
    150 			return -EBADMSG;
    151 
    152 		if (msg->param->id != SAP_PARAM_ID_COMMAND_APDU)
    153 			if ( msg->param->id != SAP_PARAM_ID_COMMAND_APDU7816)
    154 				return -EBADMSG;
    155 
    156 		if (msg->param->len == 0x00)
    157 			return -EBADMSG;
    158 
    159 		break;
    160 
    161 	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
    162 		if (msg->nparam != 0x01)
    163 			return -EBADMSG;
    164 
    165 		if (msg->param->id != SAP_PARAM_ID_TRANSPORT_PROTOCOL)
    166 			return -EBADMSG;
    167 
    168 		if (ntohs(msg->param->len) != SAP_PARAM_ID_TRANSPORT_PROTO_LEN)
    169 			return -EBADMSG;
    170 
    171 		if (*msg->param->val != SAP_TRANSPORT_PROTOCOL_T0)
    172 			if (*msg->param->val != SAP_TRANSPORT_PROTOCOL_T1)
    173 				return -EBADMSG;
    174 
    175 		break;
    176 
    177 	case SAP_DISCONNECT_REQ:
    178 	case SAP_TRANSFER_ATR_REQ:
    179 	case SAP_POWER_SIM_OFF_REQ:
    180 	case SAP_POWER_SIM_ON_REQ:
    181 	case SAP_RESET_SIM_REQ:
    182 	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
    183 		if (msg->nparam != 0x00)
    184 			return -EBADMSG;
    185 
    186 		break;
    187 	}
    188 
    189 	return 0;
    190 }
    191 
    192 static sdp_record_t *create_sap_record(uint8_t channel)
    193 {
    194 	sdp_list_t *apseq, *aproto, *profiles, *proto[2], *root, *svclass_id;
    195 	uuid_t sap_uuid, gt_uuid, root_uuid, l2cap, rfcomm;
    196 	sdp_profile_desc_t profile;
    197 	sdp_record_t *record;
    198 	sdp_data_t *ch;
    199 
    200 	record = sdp_record_alloc();
    201 	if (!record)
    202 		return NULL;
    203 
    204 	sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
    205 	root = sdp_list_append(NULL, &root_uuid);
    206 	sdp_set_browse_groups(record, root);
    207 	sdp_list_free(root, NULL);
    208 
    209 	sdp_uuid16_create(&sap_uuid, SAP_SVCLASS_ID);
    210 	svclass_id = sdp_list_append(NULL, &sap_uuid);
    211 	sdp_uuid16_create(&gt_uuid, GENERIC_TELEPHONY_SVCLASS_ID);
    212 	svclass_id = sdp_list_append(svclass_id, &gt_uuid);
    213 
    214 	sdp_set_service_classes(record, svclass_id);
    215 	sdp_list_free(svclass_id, NULL);
    216 
    217 	sdp_uuid16_create(&profile.uuid, SAP_PROFILE_ID);
    218 	profile.version = SAP_VERSION;
    219 	profiles = sdp_list_append(NULL, &profile);
    220 	sdp_set_profile_descs(record, profiles);
    221 	sdp_list_free(profiles, NULL);
    222 
    223 	sdp_uuid16_create(&l2cap, L2CAP_UUID);
    224 	proto[0] = sdp_list_append(NULL, &l2cap);
    225 	apseq = sdp_list_append(NULL, proto[0]);
    226 
    227 	sdp_uuid16_create(&rfcomm, RFCOMM_UUID);
    228 	proto[1] = sdp_list_append(NULL, &rfcomm);
    229 	ch = sdp_data_alloc(SDP_UINT8, &channel);
    230 	proto[1] = sdp_list_append(proto[1], ch);
    231 	apseq = sdp_list_append(apseq, proto[1]);
    232 
    233 	aproto = sdp_list_append(NULL, apseq);
    234 	sdp_set_access_protos(record, aproto);
    235 
    236 	sdp_set_info_attr(record, "SIM Access Server",
    237 			NULL, NULL);
    238 
    239 	sdp_data_free(ch);
    240 	sdp_list_free(proto[0], NULL);
    241 	sdp_list_free(proto[1], NULL);
    242 	sdp_list_free(apseq, NULL);
    243 	sdp_list_free(aproto, NULL);
    244 
    245 	return record;
    246 }
    247 
    248 static int send_message(struct sap_connection *conn, void *buf, size_t size)
    249 {
    250 	size_t written = 0;
    251 	GError *gerr = NULL;
    252 	GIOStatus gstatus;
    253 
    254 	if (!conn || !buf)
    255 		return -EINVAL;
    256 
    257 	DBG("size %zu", size);
    258 
    259 	gstatus = g_io_channel_write_chars(conn->io, buf, size, &written,
    260 						&gerr);
    261 	if (gstatus != G_IO_STATUS_NORMAL) {
    262 		if (gerr)
    263 			g_error_free(gerr);
    264 
    265 		error("write error (0x%02x).", gstatus);
    266 		return -EINVAL;
    267 	}
    268 
    269 	if (written != size)
    270 		error("write error.(written %zu size %zu)", written, size);
    271 
    272 	return 0;
    273 }
    274 
    275 static int disconnect_ind(void *sap_device, uint8_t disc_type)
    276 {
    277 	struct sap_connection *conn = sap_device;
    278 	char buf[SAP_BUF_SIZE];
    279 	struct sap_message *msg = (struct sap_message *) buf;
    280 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    281 	size_t size = sizeof(struct sap_message);
    282 
    283 	if (!conn)
    284 		return -EINVAL;
    285 
    286 	DBG("data %p state %d disc_type 0x%02x", conn, conn->state, disc_type);
    287 
    288 	if (conn->state != SAP_STATE_GRACEFUL_DISCONNECT &&
    289 			conn->state != SAP_STATE_IMMEDIATE_DISCONNECT) {
    290 		error("Processing error (state %d pr 0x%02x)", conn->state,
    291 							conn->processing_req);
    292 		return -EPERM;
    293 	}
    294 
    295 	memset(buf, 0, sizeof(buf));
    296 	msg->id = SAP_DISCONNECT_IND;
    297 	msg->nparam = 0x01;
    298 
    299 	/* Add disconnection type param. */
    300 	param->id  = SAP_PARAM_ID_DISCONNECT_IND;
    301 	param->len = htons(SAP_PARAM_ID_DISCONNECT_IND_LEN);
    302 	*param->val = disc_type;
    303 	size += PARAMETER_SIZE(SAP_PARAM_ID_DISCONNECT_IND_LEN);
    304 
    305 	return send_message(sap_device, buf, size);
    306 }
    307 
    308 static void connect_req(struct sap_connection *conn,
    309 				struct sap_parameter *param)
    310 {
    311 	uint16_t maxmsgsize, *val;
    312 
    313 	DBG("conn %p state %d", conn, conn->state);
    314 
    315 	if (!param)
    316 		goto error_rsp;
    317 
    318 	if (conn->state != SAP_STATE_DISCONNECTED)
    319 		goto error_rsp;
    320 
    321 	stop_guard_timer(conn);
    322 
    323 	val = (uint16_t *) &param->val;
    324 	maxmsgsize = ntohs(*val);
    325 
    326 	DBG("Connect MaxMsgSize: 0x%04x", maxmsgsize);
    327 
    328 	conn->state = SAP_STATE_CONNECT_IN_PROGRESS;
    329 
    330 	if (maxmsgsize <= SAP_BUF_SIZE) {
    331 		conn->processing_req = SAP_CONNECT_REQ;
    332 		sap_connect_req(conn, maxmsgsize);
    333 	} else {
    334 		sap_connect_rsp(conn, SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED,
    335 								SAP_BUF_SIZE);
    336 	}
    337 
    338 	return;
    339 
    340 error_rsp:
    341 	error("Processing error (param %p state %d pr 0x%02x)", param,
    342 					conn->state, conn->processing_req);
    343 	sap_error_rsp(conn);
    344 }
    345 
    346 static int disconnect_req(struct sap_connection *conn, uint8_t disc_type)
    347 {
    348 	DBG("conn %p state %d disc_type 0x%02x", conn, conn->state, disc_type);
    349 
    350 	switch (disc_type) {
    351 	case SAP_DISCONNECTION_TYPE_GRACEFUL:
    352 		if (conn->state == SAP_STATE_DISCONNECTED)
    353 			goto error_req;
    354 
    355 		if (conn->state == SAP_STATE_CONNECT_IN_PROGRESS)
    356 			goto error_req;
    357 
    358 		if (conn->state == SAP_STATE_CONNECTED) {
    359 			conn->state = SAP_STATE_GRACEFUL_DISCONNECT;
    360 			conn->processing_req = SAP_NO_REQ;
    361 
    362 			disconnect_ind(conn, disc_type);
    363 			/* Timer will disconnect if client won't do.*/
    364 			start_guard_timer(conn, SAP_TIMER_GRACEFUL_DISCONNECT);
    365 		}
    366 
    367 		return 0;
    368 
    369 	case SAP_DISCONNECTION_TYPE_IMMEDIATE:
    370 		if (conn->state == SAP_STATE_DISCONNECTED)
    371 			goto error_req;
    372 
    373 		if (conn->state == SAP_STATE_CONNECT_IN_PROGRESS)
    374 			goto error_req;
    375 
    376 		if (conn->state == SAP_STATE_CONNECTED ||
    377 				conn->state == SAP_STATE_GRACEFUL_DISCONNECT) {
    378 			conn->state = SAP_STATE_IMMEDIATE_DISCONNECT;
    379 			conn->processing_req = SAP_NO_REQ;
    380 
    381 			stop_guard_timer(conn);
    382 			disconnect_ind(conn, disc_type);
    383 			sap_disconnect_req(conn, 0);
    384 		}
    385 
    386 		return 0;
    387 
    388 	case SAP_DISCONNECTION_TYPE_CLIENT:
    389 		if (conn->state != SAP_STATE_CONNECTED &&
    390 				conn->state != SAP_STATE_GRACEFUL_DISCONNECT)
    391 			goto error_rsp;
    392 
    393 		conn->state = SAP_STATE_CLIENT_DISCONNECT;
    394 		conn->processing_req = SAP_NO_REQ;
    395 
    396 		stop_guard_timer(conn);
    397 		sap_disconnect_req(conn, 0);
    398 
    399 		return 0;
    400 
    401 	default:
    402 		error("Unknown disconnection type (0x%02x).", disc_type);
    403 		return -EINVAL;
    404 	}
    405 
    406 error_rsp:
    407 	sap_error_rsp(conn);
    408 error_req:
    409 	error("Processing error (state %d pr 0x%02x)", conn->state,
    410 						conn->processing_req);
    411 	return -EPERM;
    412 }
    413 
    414 static void transfer_apdu_req(struct sap_connection *conn,
    415 					struct sap_parameter *param)
    416 {
    417 	DBG("conn %p state %d", conn, conn->state);
    418 
    419 	if (!param)
    420 		goto error_rsp;
    421 
    422 	param->len = ntohs(param->len);
    423 
    424 	if (conn->state != SAP_STATE_CONNECTED &&
    425 			conn->state != SAP_STATE_GRACEFUL_DISCONNECT)
    426 		goto error_rsp;
    427 
    428 	if (conn->processing_req != SAP_NO_REQ)
    429 		goto error_rsp;
    430 
    431 	conn->processing_req = SAP_TRANSFER_APDU_REQ;
    432 	sap_transfer_apdu_req(conn, param);
    433 
    434 	return;
    435 
    436 error_rsp:
    437 	error("Processing error (param %p state %d pr 0x%02x)", param,
    438 					conn->state, conn->processing_req);
    439 	sap_error_rsp(conn);
    440 }
    441 
    442 static void transfer_atr_req(struct sap_connection *conn)
    443 {
    444 	DBG("conn %p state %d", conn, conn->state);
    445 
    446 	if (conn->state != SAP_STATE_CONNECTED)
    447 		goto error_rsp;
    448 
    449 	if (conn->processing_req != SAP_NO_REQ)
    450 		goto error_rsp;
    451 
    452 	conn->processing_req = SAP_TRANSFER_ATR_REQ;
    453 	sap_transfer_atr_req(conn);
    454 
    455 	return;
    456 
    457 error_rsp:
    458 	error("Processing error (state %d pr 0x%02x)", conn->state,
    459 						conn->processing_req);
    460 	sap_error_rsp(conn);
    461 }
    462 
    463 static void power_sim_off_req(struct sap_connection *conn)
    464 {
    465 	DBG("conn %p state %d", conn, conn->state);
    466 
    467 	if (conn->state != SAP_STATE_CONNECTED)
    468 		goto error_rsp;
    469 
    470 	if (!is_power_sim_off_req_allowed(conn->processing_req))
    471 		goto error_rsp;
    472 
    473 	conn->processing_req = SAP_POWER_SIM_OFF_REQ;
    474 	sap_power_sim_off_req(conn);
    475 
    476 	return;
    477 
    478 error_rsp:
    479 	error("Processing error (state %d pr 0x%02x)", conn->state,
    480 						conn->processing_req);
    481 	sap_error_rsp(conn);
    482 }
    483 
    484 static void power_sim_on_req(struct sap_connection *conn)
    485 {
    486 	DBG("conn %p state %d", conn, conn->state);
    487 
    488 	if (conn->state != SAP_STATE_CONNECTED)
    489 		goto error_rsp;
    490 
    491 	if (conn->processing_req != SAP_NO_REQ)
    492 		goto error_rsp;
    493 
    494 	conn->processing_req = SAP_POWER_SIM_ON_REQ;
    495 	sap_power_sim_on_req(conn);
    496 
    497 	return;
    498 
    499 error_rsp:
    500 	error("Processing error (state %d pr 0x%02x)", conn->state,
    501 						conn->processing_req);
    502 	sap_error_rsp(conn);
    503 }
    504 
    505 static void reset_sim_req(struct sap_connection *conn)
    506 {
    507 	DBG("conn %p state %d", conn, conn->state);
    508 
    509 	if (conn->state != SAP_STATE_CONNECTED)
    510 		goto error_rsp;
    511 
    512 	if (!is_reset_sim_req_allowed(conn->processing_req))
    513 		goto error_rsp;
    514 
    515 	conn->processing_req = SAP_RESET_SIM_REQ;
    516 	sap_reset_sim_req(conn);
    517 
    518 	return;
    519 
    520 error_rsp:
    521 	error("Processing error (state %d pr 0x%02x param)", conn->state,
    522 						conn->processing_req);
    523 	sap_error_rsp(conn);
    524 }
    525 
    526 static void transfer_card_reader_status_req(struct sap_connection *conn)
    527 {
    528 	DBG("conn %p state %d", conn, conn->state);
    529 
    530 	if (conn->state != SAP_STATE_CONNECTED)
    531 		goto error_rsp;
    532 
    533 	if (conn->processing_req != SAP_NO_REQ)
    534 		goto error_rsp;
    535 
    536 	conn->processing_req = SAP_TRANSFER_CARD_READER_STATUS_REQ;
    537 	sap_transfer_card_reader_status_req(conn);
    538 
    539 	return;
    540 
    541 error_rsp:
    542 	error("Processing error (state %d pr 0x%02x)", conn->state,
    543 						conn->processing_req);
    544 	sap_error_rsp(conn);
    545 }
    546 
    547 static void set_transport_protocol_req(struct sap_connection *conn,
    548 					struct sap_parameter *param)
    549 {
    550 	if (!param)
    551 		goto error_rsp;
    552 
    553 	DBG("conn %p state %d param %p", conn, conn->state, param);
    554 
    555 	if (conn->state != SAP_STATE_CONNECTED)
    556 		goto error_rsp;
    557 
    558 	if (conn->processing_req != SAP_NO_REQ)
    559 		goto error_rsp;
    560 
    561 	conn->processing_req = SAP_SET_TRANSPORT_PROTOCOL_REQ;
    562 	sap_set_transport_protocol_req(conn, param);
    563 
    564 	return;
    565 
    566 error_rsp:
    567 	error("Processing error (param %p state %d pr 0x%02x)", param,
    568 					conn->state, conn->processing_req);
    569 	sap_error_rsp(conn);
    570 }
    571 
    572 static void start_guard_timer(struct sap_connection *conn, guint interval)
    573 {
    574 	if (!conn)
    575 		return;
    576 
    577 	if (!conn->timer_id)
    578 		conn->timer_id = g_timeout_add_seconds(interval, guard_timeout,
    579 									conn);
    580 	else
    581 		error("Timer is already active.");
    582 }
    583 
    584 static void stop_guard_timer(struct sap_connection *conn)
    585 {
    586 	if (conn  && conn->timer_id) {
    587 		g_source_remove(conn->timer_id);
    588 		conn->timer_id = 0;
    589 	}
    590 }
    591 
    592 static gboolean guard_timeout(gpointer data)
    593 {
    594 	struct sap_connection *conn = data;
    595 
    596 	if (!conn)
    597 		return FALSE;
    598 
    599 	DBG("conn %p state %d pr 0x%02x", conn, conn->state,
    600 						conn->processing_req);
    601 
    602 	conn->timer_id = 0;
    603 
    604 	switch (conn->state) {
    605 	case SAP_STATE_DISCONNECTED:
    606 		/* Client opened RFCOMM channel but didn't send CONNECT_REQ,
    607 		 * in fixed time or client disconnected SAP connection but
    608 		 * didn't closed RFCOMM channel in fixed time.*/
    609 		if (conn->io) {
    610 			g_io_channel_shutdown(conn->io, TRUE, NULL);
    611 			g_io_channel_unref(conn->io);
    612 		}
    613 		break;
    614 
    615 	case SAP_STATE_GRACEFUL_DISCONNECT:
    616 		/* Client didn't disconnect SAP connection in fixed time,
    617 		 * so close SAP connection immediately. */
    618 		disconnect_req(conn, SAP_DISCONNECTION_TYPE_IMMEDIATE);
    619 		break;
    620 
    621 	default:
    622 		error("Unexpected state (%d).", conn->state);
    623 		break;
    624 	}
    625 
    626 	return FALSE;
    627 }
    628 
    629 int sap_connect_rsp(void *sap_device, uint8_t status, uint16_t maxmsgsize)
    630 {
    631 	struct sap_connection *conn = sap_device;
    632 	char buf[SAP_BUF_SIZE];
    633 	struct sap_message *msg = (struct sap_message *) buf;
    634 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    635 	size_t size = sizeof(struct sap_message);
    636 
    637 	if (!conn)
    638 		return -EINVAL;
    639 
    640 	DBG("state %d pr 0x%02x status 0x%02x", conn->state,
    641 						conn->processing_req, status);
    642 
    643 	if (conn->state != SAP_STATE_CONNECT_IN_PROGRESS)
    644 		return -EPERM;
    645 
    646 	memset(buf, 0, sizeof(buf));
    647 	msg->id = SAP_CONNECT_RESP;
    648 	msg->nparam = 0x01;
    649 
    650 	/* Add connection status */
    651 	param->id = SAP_PARAM_ID_CONN_STATUS;
    652 	param->len = htons(SAP_PARAM_ID_CONN_STATUS_LEN);
    653 	*param->val = status;
    654 	size += PARAMETER_SIZE(SAP_PARAM_ID_CONN_STATUS_LEN);
    655 
    656 	/* Add MaxMsgSize */
    657 	if (maxmsgsize && (status == SAP_STATUS_MAX_MSG_SIZE_NOT_SUPPORTED ||
    658 				status == SAP_STATUS_MAX_MSG_SIZE_TOO_SMALL)) {
    659 		uint16_t *len;
    660 
    661 		msg->nparam++;
    662 		param = (struct sap_parameter *) &buf[size];
    663 		param->id = SAP_PARAM_ID_MAX_MSG_SIZE;
    664 		param->len = htons(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
    665 		len = (uint16_t *) &param->val;
    666 		*len = htons(maxmsgsize);
    667 		size += PARAMETER_SIZE(SAP_PARAM_ID_MAX_MSG_SIZE_LEN);
    668 	}
    669 
    670 	if (status == SAP_STATUS_OK) {
    671 		gboolean connected = TRUE;
    672 
    673 		emit_property_changed(connection, server->path,
    674 						SAP_SERVER_INTERFACE,
    675 			"Connected", DBUS_TYPE_BOOLEAN, &connected);
    676 
    677 		conn->state = SAP_STATE_CONNECTED;
    678 	} else {
    679 		conn->state = SAP_STATE_DISCONNECTED;
    680 
    681 		/* Timer will shutdown channel if client doesn't send
    682 		 * CONNECT_REQ or doesn't shutdown channel itself.*/
    683 		start_guard_timer(conn, SAP_TIMER_NO_ACTIVITY);
    684 	}
    685 
    686 	conn->processing_req = SAP_NO_REQ;
    687 
    688 	return send_message(sap_device, buf, size);
    689 }
    690 
    691 int sap_disconnect_rsp(void *sap_device)
    692 {
    693 	struct sap_connection *conn = sap_device;
    694 	struct sap_message msg;
    695 
    696 	if (!conn)
    697 		return -EINVAL;
    698 
    699 	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
    700 
    701 	switch (conn->state) {
    702 	case SAP_STATE_CLIENT_DISCONNECT:
    703 		memset(&msg, 0, sizeof(msg));
    704 		msg.id = SAP_DISCONNECT_RESP;
    705 
    706 		conn->state = SAP_STATE_DISCONNECTED;
    707 		conn->processing_req = SAP_NO_REQ;
    708 
    709 		/* Timer will close channel if client doesn't do it.*/
    710 		start_guard_timer(conn, SAP_TIMER_NO_ACTIVITY);
    711 
    712 		return send_message(sap_device, &msg, sizeof(msg));
    713 
    714 	case SAP_STATE_IMMEDIATE_DISCONNECT:
    715 		conn->state = SAP_STATE_DISCONNECTED;
    716 		conn->processing_req = SAP_NO_REQ;
    717 
    718 		if (conn->io) {
    719 			g_io_channel_shutdown(conn->io, TRUE, NULL);
    720 			g_io_channel_unref(conn->io);
    721 		}
    722 
    723 		return 0;
    724 
    725 	default:
    726 		break;
    727 	}
    728 
    729 	return 0;
    730 }
    731 
    732 int sap_transfer_apdu_rsp(void *sap_device, uint8_t result, uint8_t *apdu,
    733 					uint16_t length)
    734 {
    735 	struct sap_connection *conn = sap_device;
    736 	char buf[SAP_BUF_SIZE];
    737 	struct sap_message *msg = (struct sap_message *) buf;
    738 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    739 	size_t size = sizeof(struct sap_message);
    740 
    741 	if (!conn)
    742 		return -EINVAL;
    743 
    744 	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
    745 
    746 	if (conn->processing_req != SAP_TRANSFER_APDU_REQ)
    747 		return 0;
    748 
    749 	if (result == SAP_RESULT_OK && (!apdu || (apdu && length == 0x00)))
    750 		return -EINVAL;
    751 
    752 	memset(buf, 0, sizeof(buf));
    753 	msg->id = SAP_TRANSFER_APDU_RESP;
    754 	msg->nparam = 0x01;
    755 	size += add_result_parameter(result, param);
    756 
    757 	/* Add APDU response. */
    758 	if (result == SAP_RESULT_OK) {
    759 		msg->nparam++;
    760 		param = (struct sap_parameter *) &buf[size];
    761 		param->id = SAP_PARAM_ID_RESPONSE_APDU;
    762 		param->len = htons(length);
    763 
    764 		size += PARAMETER_SIZE(length);
    765 
    766 		if (size > SAP_BUF_SIZE)
    767 			return -EOVERFLOW;
    768 
    769 		memcpy(param->val, apdu, length);
    770 	}
    771 
    772 	conn->processing_req = SAP_NO_REQ;
    773 
    774 	return send_message(sap_device, buf, size);
    775 }
    776 
    777 int sap_transfer_atr_rsp(void *sap_device, uint8_t result, uint8_t *atr,
    778 					uint16_t length)
    779 {
    780 	struct sap_connection *conn = sap_device;
    781 	char buf[SAP_BUF_SIZE];
    782 	struct sap_message *msg = (struct sap_message *) buf;
    783 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    784 	size_t size = sizeof(struct sap_message);
    785 
    786 	if (!conn)
    787 		return -EINVAL;
    788 
    789 	DBG("result 0x%02x state %d pr 0x%02x len %d", result, conn->state,
    790 			conn->processing_req, length);
    791 
    792 	if (conn->processing_req != SAP_TRANSFER_ATR_REQ)
    793 		return 0;
    794 
    795 	if (result == SAP_RESULT_OK && (!atr || (atr && length == 0x00)))
    796 		return -EINVAL;
    797 
    798 	memset(buf, 0, sizeof(buf));
    799 	msg->id = SAP_TRANSFER_ATR_RESP;
    800 	msg->nparam = 0x01;
    801 	size += add_result_parameter(result, param);
    802 
    803 	/* Add ATR response */
    804 	if (result == SAP_RESULT_OK) {
    805 		msg->nparam++;
    806 		param = (struct sap_parameter *) &buf[size];
    807 		param->id = SAP_PARAM_ID_ATR;
    808 		param->len = htons(length);
    809 		size += PARAMETER_SIZE(length);
    810 
    811 		if (size > SAP_BUF_SIZE)
    812 			return -EOVERFLOW;
    813 
    814 		memcpy(param->val, atr, length);
    815 	}
    816 
    817 	conn->processing_req = SAP_NO_REQ;
    818 
    819 	return send_message(sap_device, buf, size);
    820 }
    821 
    822 int sap_power_sim_off_rsp(void *sap_device, uint8_t result)
    823 {
    824 	struct sap_connection *conn = sap_device;
    825 	char buf[SAP_BUF_SIZE];
    826 	struct sap_message *msg = (struct sap_message *) buf;
    827 	size_t size = sizeof(struct sap_message);
    828 
    829 	if (!conn)
    830 		return -EINVAL;
    831 
    832 	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
    833 
    834 	if (conn->processing_req != SAP_POWER_SIM_OFF_REQ)
    835 		return 0;
    836 
    837 	memset(buf, 0, sizeof(buf));
    838 	msg->id = SAP_POWER_SIM_OFF_RESP;
    839 	msg->nparam = 0x01;
    840 	size += add_result_parameter(result, msg->param);
    841 
    842 	conn->processing_req = SAP_NO_REQ;
    843 
    844 	return send_message(sap_device, buf, size);
    845 }
    846 
    847 int sap_power_sim_on_rsp(void *sap_device, uint8_t result)
    848 {
    849 	struct sap_connection *conn = sap_device;
    850 	char buf[SAP_BUF_SIZE];
    851 	struct sap_message *msg = (struct sap_message *) buf;
    852 	size_t size = sizeof(struct sap_message);
    853 
    854 	if (!conn)
    855 		return -EINVAL;
    856 
    857 	DBG("state %d pr 0x%02x", conn->state, conn->processing_req);
    858 
    859 	if (conn->processing_req != SAP_POWER_SIM_ON_REQ)
    860 		return 0;
    861 
    862 	memset(buf, 0, sizeof(buf));
    863 	msg->id = SAP_POWER_SIM_ON_RESP;
    864 	msg->nparam = 0x01;
    865 	size += add_result_parameter(result, msg->param);
    866 
    867 	conn->processing_req = SAP_NO_REQ;
    868 
    869 	return send_message(sap_device, buf, size);
    870 }
    871 
    872 int sap_reset_sim_rsp(void *sap_device, uint8_t result)
    873 {
    874 	struct sap_connection *conn = sap_device;
    875 	char buf[SAP_BUF_SIZE];
    876 	struct sap_message *msg = (struct sap_message *) buf;
    877 	size_t size = sizeof(struct sap_message);
    878 
    879 	if (!conn)
    880 		return -EINVAL;
    881 
    882 	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
    883 					conn->processing_req, result);
    884 
    885 	if (conn->processing_req != SAP_RESET_SIM_REQ)
    886 		return 0;
    887 
    888 	memset(buf, 0, sizeof(buf));
    889 	msg->id = SAP_RESET_SIM_RESP;
    890 	msg->nparam = 0x01;
    891 	size += add_result_parameter(result, msg->param);
    892 
    893 	conn->processing_req = SAP_NO_REQ;
    894 
    895 	return send_message(sap_device, buf, size);
    896 }
    897 
    898 int sap_transfer_card_reader_status_rsp(void *sap_device, uint8_t result,
    899 						uint8_t status)
    900 {
    901 	struct sap_connection *conn = sap_device;
    902 	char buf[SAP_BUF_SIZE];
    903 	struct sap_message *msg = (struct sap_message *) buf;
    904 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    905 	size_t size = sizeof(struct sap_message);
    906 
    907 	if (!conn)
    908 		return -EINVAL;
    909 
    910 	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
    911 					conn->processing_req, result);
    912 
    913 	if (conn->processing_req != SAP_TRANSFER_CARD_READER_STATUS_REQ)
    914 		return 0;
    915 
    916 	memset(buf, 0, sizeof(buf));
    917 	msg->id = SAP_TRANSFER_CARD_READER_STATUS_RESP;
    918 	msg->nparam = 0x01;
    919 	size += add_result_parameter(result, param);
    920 
    921 	/* Add card reader status. */
    922 	if (result == SAP_RESULT_OK) {
    923 		msg->nparam++;
    924 		param = (struct sap_parameter *) &buf[size];
    925 		param->id = SAP_PARAM_ID_CARD_READER_STATUS;
    926 		param->len = htons(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
    927 		*param->val = status;
    928 		size += PARAMETER_SIZE(SAP_PARAM_ID_CARD_READER_STATUS_LEN);
    929 	}
    930 
    931 	conn->processing_req = SAP_NO_REQ;
    932 
    933 	return send_message(sap_device, buf, size);
    934 }
    935 
    936 int sap_transport_protocol_rsp(void *sap_device, uint8_t result)
    937 {
    938 	struct sap_connection *conn = sap_device;
    939 	char buf[SAP_BUF_SIZE];
    940 	struct sap_message *msg = (struct sap_message *) buf;
    941 	size_t size = sizeof(struct sap_message);
    942 
    943 	if (!conn)
    944 		return -EINVAL;
    945 
    946 	DBG("state %d pr 0x%02x result 0x%02x", conn->state,
    947 					conn->processing_req, result);
    948 
    949 	if (conn->processing_req != SAP_SET_TRANSPORT_PROTOCOL_REQ)
    950 		return 0;
    951 
    952 	memset(buf, 0, sizeof(buf));
    953 	msg->id = SAP_SET_TRANSPORT_PROTOCOL_RESP;
    954 	msg->nparam = 0x01;
    955 	size += add_result_parameter(result, msg->param);
    956 
    957 	conn->processing_req = SAP_NO_REQ;
    958 
    959 	return send_message(sap_device, buf, size);
    960 }
    961 
    962 int sap_error_rsp(void *sap_device)
    963 {
    964 	struct sap_message msg;
    965 	struct sap_connection *conn = sap_device;
    966 
    967 	memset(&msg, 0, sizeof(msg));
    968 	msg.id = SAP_ERROR_RESP;
    969 
    970 	return send_message(conn, &msg, sizeof(msg));
    971 }
    972 
    973 int sap_status_ind(void *sap_device, uint8_t status_change)
    974 {
    975 	struct sap_connection *conn = sap_device;
    976 	char buf[SAP_BUF_SIZE];
    977 	struct sap_message *msg = (struct sap_message *) buf;
    978 	struct sap_parameter *param = (struct sap_parameter *) msg->param;
    979 	size_t size = sizeof(struct sap_message);
    980 
    981 	if (!conn)
    982 		return -EINVAL;
    983 
    984 	DBG("state %d pr 0x%02x sc 0x%02x", conn->state, conn->processing_req,
    985 				status_change);
    986 
    987 	if (conn->state != SAP_STATE_CONNECTED &&
    988 			conn->state != SAP_STATE_GRACEFUL_DISCONNECT)
    989 		return 0;
    990 
    991 	memset(buf, 0, sizeof(buf));
    992 	msg->id = SAP_STATUS_IND;
    993 	msg->nparam = 0x01;
    994 
    995 	/* Add status change. */
    996 	param->id  = SAP_PARAM_ID_STATUS_CHANGE;
    997 	param->len = htons(SAP_PARAM_ID_STATUS_CHANGE_LEN);
    998 	*param->val = status_change;
    999 	size += PARAMETER_SIZE(SAP_PARAM_ID_STATUS_CHANGE_LEN);
   1000 
   1001 	return send_message(sap_device, buf, size);
   1002 }
   1003 
   1004 int sap_disconnect_ind(void *sap_device, uint8_t disc_type)
   1005 {
   1006 	struct sap_connection *conn = sap_device;
   1007 
   1008 	return disconnect_req(conn, SAP_DISCONNECTION_TYPE_IMMEDIATE);
   1009 }
   1010 
   1011 static int handle_cmd(void *data, void *buf, size_t size)
   1012 {
   1013 	struct sap_message *msg = buf;
   1014 	struct sap_connection *conn = data;
   1015 
   1016 	if (!conn)
   1017 		return -EINVAL;
   1018 
   1019 	if (size < sizeof(struct sap_message))
   1020 		goto error_rsp;
   1021 
   1022 	if (msg->nparam != 0 && size < (sizeof(struct sap_message) +
   1023 					sizeof(struct sap_parameter) + 4))
   1024 		goto error_rsp;
   1025 
   1026 	if (check_msg(msg) < 0)
   1027 		goto error_rsp;
   1028 
   1029 	switch (msg->id) {
   1030 	case SAP_CONNECT_REQ:
   1031 		connect_req(conn, msg->param);
   1032 		return 0;
   1033 	case SAP_DISCONNECT_REQ:
   1034 		disconnect_req(conn, SAP_DISCONNECTION_TYPE_CLIENT);
   1035 		return 0;
   1036 	case SAP_TRANSFER_APDU_REQ:
   1037 		transfer_apdu_req(conn, msg->param);
   1038 		return 0;
   1039 	case SAP_TRANSFER_ATR_REQ:
   1040 		transfer_atr_req(conn);
   1041 		return 0;
   1042 	case SAP_POWER_SIM_OFF_REQ:
   1043 		power_sim_off_req(conn);
   1044 		return 0;
   1045 	case SAP_POWER_SIM_ON_REQ:
   1046 		power_sim_on_req(conn);
   1047 		return 0;
   1048 	case SAP_RESET_SIM_REQ:
   1049 		reset_sim_req(conn);
   1050 		return 0;
   1051 	case SAP_TRANSFER_CARD_READER_STATUS_REQ:
   1052 		transfer_card_reader_status_req(conn);
   1053 		return 0;
   1054 	case SAP_SET_TRANSPORT_PROTOCOL_REQ:
   1055 		set_transport_protocol_req(conn, msg->param);
   1056 		return 0;
   1057 	default:
   1058 		DBG("SAP unknown message.");
   1059 		break;
   1060 	}
   1061 
   1062 error_rsp:
   1063 	DBG("Bad request message format.");
   1064 	sap_error_rsp(conn);
   1065 	return -EBADMSG;
   1066 }
   1067 
   1068 static void sap_conn_remove(struct sap_connection *conn)
   1069 {
   1070 	DBG("conn %p", conn);
   1071 
   1072 	if (!conn)
   1073 		return;
   1074 
   1075 	if (conn->io) {
   1076 		g_io_channel_shutdown(conn->io, TRUE, NULL);
   1077 		g_io_channel_unref(conn->io);
   1078 	}
   1079 
   1080 	conn->io = NULL;
   1081 	g_free(conn);
   1082 	server->conn = NULL;
   1083 }
   1084 
   1085 static gboolean sap_io_cb(GIOChannel *io, GIOCondition cond, gpointer data)
   1086 {
   1087 	char buf[SAP_BUF_SIZE];
   1088 	size_t bytes_read = 0;
   1089 	GError *gerr = NULL;
   1090 	GIOStatus gstatus;
   1091 
   1092 	DBG("io %p", io);
   1093 
   1094 	if (cond & G_IO_NVAL) {
   1095 		DBG("ERR (G_IO_NVAL) on rfcomm socket.");
   1096 		return FALSE;
   1097 	}
   1098 
   1099 	if (cond & G_IO_ERR) {
   1100 		DBG("ERR (G_IO_ERR) on rfcomm socket.");
   1101 		return FALSE;
   1102 	}
   1103 
   1104 	if (cond & G_IO_HUP) {
   1105 		DBG("HUP on rfcomm socket.");
   1106 		return FALSE;
   1107 	}
   1108 
   1109 	gstatus = g_io_channel_read_chars(io, buf, sizeof(buf) - 1,
   1110 				&bytes_read, &gerr);
   1111 	if (gstatus != G_IO_STATUS_NORMAL) {
   1112 		if (gerr)
   1113 			g_error_free(gerr);
   1114 
   1115 		return TRUE;
   1116 	}
   1117 
   1118 	if (handle_cmd(data, buf, bytes_read) < 0)
   1119 		error("Invalid SAP message.");
   1120 
   1121 	return TRUE;
   1122 }
   1123 
   1124 static void sap_io_destroy(void *data)
   1125 {
   1126 	struct sap_connection *conn = data;
   1127 
   1128 	DBG("conn %p", conn);
   1129 
   1130 	if (conn && conn->io) {
   1131 		gboolean connected = FALSE;
   1132 
   1133 		stop_guard_timer(conn);
   1134 
   1135 		if (conn->state != SAP_STATE_CONNECT_IN_PROGRESS)
   1136 			emit_property_changed(connection, server->path,
   1137 					SAP_SERVER_INTERFACE, "Connected",
   1138 					DBUS_TYPE_BOOLEAN, &connected);
   1139 
   1140 		if (conn->state == SAP_STATE_CONNECT_IN_PROGRESS ||
   1141 				conn->state == SAP_STATE_CONNECTED ||
   1142 				conn->state == SAP_STATE_GRACEFUL_DISCONNECT)
   1143 			sap_disconnect_req(NULL, 1);
   1144 
   1145 		conn->io = NULL;
   1146 		sap_conn_remove(conn);
   1147 	}
   1148 }
   1149 
   1150 static void sap_connect_cb(GIOChannel *io, GError *gerr, gpointer data)
   1151 {
   1152 	struct sap_connection *conn = data;
   1153 
   1154 	DBG("io %p gerr %p data %p ", io, gerr, data);
   1155 
   1156 	if (!conn)
   1157 		return;
   1158 
   1159 	/* Timer will shutdown the channel in case of lack of client
   1160 	   activity */
   1161 	start_guard_timer(conn, SAP_TIMER_NO_ACTIVITY);
   1162 
   1163 	g_io_add_watch_full(io, G_PRIORITY_DEFAULT,
   1164 			G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
   1165 			sap_io_cb, conn, sap_io_destroy);
   1166 }
   1167 
   1168 static void connect_auth_cb(DBusError *derr, void *data)
   1169 {
   1170 	struct sap_connection *conn = data;
   1171 	GError *gerr = NULL;
   1172 
   1173 	DBG("derr %p data %p ", derr, data);
   1174 
   1175 	if (!conn)
   1176 		return;
   1177 
   1178 	if (derr && dbus_error_is_set(derr)) {
   1179 		error("Access denied: %s", derr->message);
   1180 		sap_conn_remove(conn);
   1181 		return;
   1182 	}
   1183 
   1184 	if (!bt_io_accept(conn->io, sap_connect_cb, conn, NULL, &gerr)) {
   1185 		error("bt_io_accept: %s", gerr->message);
   1186 		g_error_free(gerr);
   1187 		sap_conn_remove(conn);
   1188 		return;
   1189 	}
   1190 
   1191 	DBG("Client has been authorized.");
   1192 }
   1193 
   1194 static void connect_confirm_cb(GIOChannel *io, gpointer data)
   1195 {
   1196 	struct sap_connection *conn = server->conn;
   1197 	GError *gerr = NULL;
   1198 	bdaddr_t src, dst;
   1199 	int err;
   1200 
   1201 	DBG("io %p data %p ", io, data);
   1202 
   1203 	if (!io)
   1204 		return;
   1205 
   1206 	if (conn) {
   1207 		g_io_channel_shutdown(io, TRUE, NULL);
   1208 		return;
   1209 	}
   1210 
   1211 	conn = g_try_new0(struct sap_connection, 1);
   1212 	if (!conn) {
   1213 		error("Can't allocate memory for incomming SAP connection.");
   1214 		g_io_channel_shutdown(io, TRUE, NULL);
   1215 		return;
   1216 	}
   1217 
   1218 	g_io_channel_set_encoding(io, NULL, NULL);
   1219 	g_io_channel_set_buffered(io, FALSE);
   1220 
   1221 	server->conn = conn;
   1222 	conn->io = g_io_channel_ref(io);
   1223 	conn->state = SAP_STATE_DISCONNECTED;
   1224 
   1225 	bt_io_get(io, BT_IO_RFCOMM, &gerr,
   1226 			BT_IO_OPT_SOURCE_BDADDR, &src,
   1227 			BT_IO_OPT_DEST_BDADDR, &dst,
   1228 			BT_IO_OPT_INVALID);
   1229 	if (gerr) {
   1230 		error("%s", gerr->message);
   1231 		g_error_free(gerr);
   1232 		sap_conn_remove(conn);
   1233 		return;
   1234 	}
   1235 
   1236 	err = btd_request_authorization(&src, &dst, SAP_UUID,
   1237 					connect_auth_cb, conn);
   1238 	if (err < 0) {
   1239 		DBG("Authorization denied: %d %s", err,  strerror(err));
   1240 		sap_conn_remove(conn);
   1241 		return;
   1242 	}
   1243 
   1244 	DBG("SAP incoming connection (sock %d) authorization.",
   1245 				g_io_channel_unix_get_fd(io));
   1246 }
   1247 
   1248 static inline DBusMessage *message_failed(DBusMessage *msg,
   1249 					const char *description)
   1250 {
   1251 	return g_dbus_create_error(msg, ERROR_INTERFACE ".Failed",
   1252 				"%s", description);
   1253 }
   1254 
   1255 static DBusMessage *disconnect(DBusConnection *conn, DBusMessage *msg,
   1256 								void *data)
   1257 {
   1258 	struct sap_server *server = data;
   1259 
   1260 	DBG("server %p", server);
   1261 
   1262 	if (!server)
   1263 		return message_failed(msg, "Server internal error.");
   1264 
   1265 	DBG("conn %p", server->conn);
   1266 
   1267 	if (!server->conn)
   1268 		return message_failed(msg, "Client already disconnected");
   1269 
   1270 	if (disconnect_req(server->conn, SAP_DISCONNECTION_TYPE_GRACEFUL) < 0)
   1271 		return g_dbus_create_error(msg, ERROR_INTERFACE	".Failed",
   1272 				"There is no active connection");
   1273 
   1274 	return dbus_message_new_method_return(msg);
   1275 }
   1276 
   1277 static DBusMessage *get_properties(DBusConnection *c,
   1278 				DBusMessage *msg, void *data)
   1279 {
   1280 	struct sap_connection *conn = data;
   1281 	DBusMessage *reply;
   1282 	DBusMessageIter iter;
   1283 	DBusMessageIter dict;
   1284 	dbus_bool_t connected;
   1285 
   1286 	if (!conn)
   1287 		return message_failed(msg, "Server internal error.");
   1288 
   1289 	reply = dbus_message_new_method_return(msg);
   1290 	if (!reply)
   1291 		return NULL;
   1292 
   1293 	dbus_message_iter_init_append(reply, &iter);
   1294 
   1295 	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
   1296 			DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
   1297 			DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING
   1298 			DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
   1299 
   1300 	connected = (conn->state == SAP_STATE_CONNECTED ||
   1301 			conn->state == SAP_STATE_GRACEFUL_DISCONNECT);
   1302 	dict_append_entry(&dict, "Connected", DBUS_TYPE_BOOLEAN, &connected);
   1303 
   1304 	dbus_message_iter_close_container(&iter, &dict);
   1305 
   1306 	return reply;
   1307 }
   1308 
   1309 static GDBusMethodTable server_methods[] = {
   1310 	{"GetProperties", "", "a{sv}", get_properties},
   1311 	{"Disconnect", "", "", disconnect},
   1312 	{ }
   1313 };
   1314 
   1315 static GDBusSignalTable server_signals[] = {
   1316 	{ "PropertyChanged", "sv"},
   1317 	{ }
   1318 };
   1319 
   1320 static void server_free(struct sap_server *server)
   1321 {
   1322 	if (!server)
   1323 		return;
   1324 
   1325 	sap_conn_remove(server->conn);
   1326 	g_free(server->path);
   1327 	g_free(server);
   1328 	server = NULL;
   1329 }
   1330 
   1331 static void destroy_sap_interface(void *data)
   1332 {
   1333 	struct sap_server *server = data;
   1334 
   1335 	DBG("Unregistered interface %s on path %s",
   1336 			SAP_SERVER_INTERFACE, server->path);
   1337 
   1338 	server_free(server);
   1339 }
   1340 
   1341 int sap_server_register(const char *path, bdaddr_t *src)
   1342 {
   1343 	sdp_record_t *record = NULL;
   1344 	GError *gerr = NULL;
   1345 	GIOChannel *io;
   1346 
   1347 	if (sap_init() < 0) {
   1348 		error("Sap driver initialization failed.");
   1349 		return -1;
   1350 	}
   1351 
   1352 	server = g_try_new0(struct sap_server, 1);
   1353 	if (!server) {
   1354 		sap_exit();
   1355 		return -ENOMEM;
   1356 	}
   1357 
   1358 	bacpy(&server->src, src);
   1359 	server->path = g_strdup(path);
   1360 
   1361 	record = create_sap_record(SAP_SERVER_CHANNEL);
   1362 	if (!record) {
   1363 		error("Creating SAP SDP record failed.");
   1364 		goto sdp_err;
   1365 	}
   1366 
   1367 	if (add_record_to_server(&server->src, record) < 0) {
   1368 		error("Adding SAP SDP record to the SDP server failed.");
   1369 		sdp_record_free(record);
   1370 		goto sdp_err;
   1371 	}
   1372 
   1373 	server->record_id = record->handle;
   1374 
   1375 	io = bt_io_listen(BT_IO_RFCOMM, NULL, connect_confirm_cb, server,
   1376 			NULL, &gerr,
   1377 			BT_IO_OPT_SOURCE_BDADDR, &server->src,
   1378 			BT_IO_OPT_CHANNEL, SAP_SERVER_CHANNEL,
   1379 			BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_HIGH,
   1380 			BT_IO_OPT_MASTER, TRUE,
   1381 			BT_IO_OPT_INVALID);
   1382 	if (!io) {
   1383 		error("Can't listen at channel %d.", SAP_SERVER_CHANNEL);
   1384 		g_error_free(gerr);
   1385 		goto server_err;
   1386 	}
   1387 
   1388 	DBG("Listen socket 0x%02x", g_io_channel_unix_get_fd(io));
   1389 
   1390 	server->listen_io = io;
   1391 	server->conn = NULL;
   1392 
   1393 	if (!g_dbus_register_interface(connection, path, SAP_SERVER_INTERFACE,
   1394 				server_methods, server_signals, NULL,
   1395 				server, destroy_sap_interface)) {
   1396 		error("D-Bus failed to register %s interface",
   1397 						SAP_SERVER_INTERFACE);
   1398 		goto server_err;
   1399 	}
   1400 
   1401 	return 0;
   1402 
   1403 server_err:
   1404 	remove_record_from_server(server->record_id);
   1405 sdp_err:
   1406 	server_free(server);
   1407 	sap_exit();
   1408 
   1409 	return -1;
   1410 }
   1411 
   1412 int sap_server_unregister(const char *path)
   1413 {
   1414 	if (!server)
   1415 		return -EINVAL;
   1416 
   1417 	remove_record_from_server(server->record_id);
   1418 
   1419 	if (server->conn)
   1420 		sap_conn_remove(server->conn);
   1421 
   1422 	if (server->listen_io) {
   1423 		g_io_channel_shutdown(server->listen_io, TRUE, NULL);
   1424 		g_io_channel_unref(server->listen_io);
   1425 		server->listen_io = NULL;
   1426 	}
   1427 
   1428 	g_dbus_unregister_interface(connection, path, SAP_SERVER_INTERFACE);
   1429 
   1430 	sap_exit();
   1431 
   1432 	return 0;
   1433 }
   1434 
   1435 int sap_server_init(DBusConnection *conn)
   1436 {
   1437 	connection = dbus_connection_ref(conn);
   1438 	return 0;
   1439 }
   1440 
   1441 void sap_server_exit(void)
   1442 {
   1443 	dbus_connection_unref(connection);
   1444 	connection = NULL;
   1445 }
   1446