source: asp3_tinet_ecnl_rx/trunk/bnep_bridge/src/panu_main.c@ 337

Last change on this file since 337 was 337, checked in by coas-nagasima, 6 years ago

ASP3版ECNLを追加
  • Property svn:eol-style set to native
  • Property svn:mime-type set to text/x-csrc;charset=UTF-8
File size: 18.0 KB
Line 
1/*
2 * Copyright (C) 2014 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 3. Neither the name of the copyright holders nor the names of
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 * 4. Any redistribution, use, or modification is done solely for
17 * personal benefit and not for any commercial purpose or for
18 * monetary gain.
19 *
20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 *
33 * Please inquire about commercial licensing options at
34 * contact@bluekitchen-gmbh.com
35 *
36 */
37
38#include "btstack-config.h"
39
40#include <stdint.h>
41
42#include <btstack/hci_cmds.h>
43#include <btstack/run_loop.h>
44#include <btstack/sdp_util.h>
45
46#include <kernel.h>
47#include "kernel_cfg.h"
48#include "target_syssvc.h"
49
50#include "debug.h"
51#include "hci.h"
52#include "btstack_memory.h"
53#include "hci_dump.h"
54#include "l2cap.h"
55#include "sdp_parser.h"
56#include "sdp_client.h"
57#include "sdp_query_util.h"
58#include "sdp.h"
59#include "pan.h"
60#include "panu_main.h"
61
62//#define PACKET_DUMP
63
64// エラー返却用定数(-1:0xffff固定)
65#define FAILED_RFCOMM_ID_NOT_SET 0xffff
66
67#define RFCOMM_SERVER_CHANNEL 1
68/*static*/ uint16_t rfcomm_channel_id = FAILED_RFCOMM_ID_NOT_SET;
69
70static int record_id = -1;
71static uint16_t bnep_l2cap_psm = 0;
72static uint32_t bnep_remote_uuid = 0;
73static uint16_t bnep_version = 0;
74static uint16_t bnep_cid = 0;
75static bool_t bnep_do_connect = false;
76
77static uint16_t sdp_bnep_l2cap_psm = 0;
78static uint16_t sdp_bnep_version = 0;
79static uint32_t sdp_bnep_remote_uuid = 0;
80
81uint8_t attribute_value[1000];
82const unsigned int attribute_value_buffer_size = sizeof(attribute_value);
83
84extern bd_addr_t remote_addr;
85
86uint8_t spp_service_buffer[120];
87uint8_t bnep_service_buffer[256];
88
89char bt_localname[] = TARGET_NAME"0000";
90
91volatile int bt_bnep_mode = 0;
92bool_t bt_enabled = false;
93
94// outgoing network packet
95const uint8_t * network_buffer;
96uint16_t network_buffer_len;
97uint16_t network_buffer_cid;
98
99/*************** PANU client routines *********************/
100
101char * get_string_from_data_element(uint8_t * element) {
102 de_size_t de_size = de_get_size_type(element);
103 int pos = de_get_header_size(element);
104 int len = 0;
105 switch (de_size) {
106 case DE_SIZE_VAR_8:
107 len = element[1];
108 break;
109 case DE_SIZE_VAR_16:
110 len = READ_NET_16(element, 1);
111 break;
112 default:
113 break;
114 }
115 char * str = (char*)malloc(len + 1);
116 memcpy(str, &element[pos], len);
117 str[len] = '\0';
118 return str;
119}
120
121/* @section SDP parser callback
122*
123* @text The SDP parsers retrieves the BNEP PAN UUID as explained in
124* Section [on SDP BNEP Query example](#sec:sdpbnepqueryExample}.
125*/
126static void handle_sdp_client_record_complete(void)
127{
128 log_info("SDP BNEP Record complete");
129
130 // accept first entry or if we foudn a NAP and only have a PANU yet
131 if ((bnep_remote_uuid == 0) || (sdp_bnep_remote_uuid == BNEP_UUID_NAP && bnep_remote_uuid == BNEP_UUID_PANU)) {
132 bnep_l2cap_psm = sdp_bnep_l2cap_psm;
133 bnep_remote_uuid = sdp_bnep_remote_uuid;
134 bnep_version = sdp_bnep_version;
135 }
136}
137
138/* SDP parser callback */
139static void handle_sdp_client_query_result(sdp_query_event_t *event)
140{
141 sdp_query_attribute_value_event_t *value_event;
142 sdp_query_complete_event_t *complete_event;
143 des_iterator_t des_list_it;
144 des_iterator_t prot_it;
145 char *str;
146
147 switch (event->type) {
148 case SDP_QUERY_ATTRIBUTE_VALUE:
149 value_event = (sdp_query_attribute_value_event_t*)event;
150
151 // Handle new SDP record
152 if (value_event->record_id != record_id) {
153 handle_sdp_client_record_complete();
154 // next record started
155 record_id = value_event->record_id;
156 log_info("SDP Record: Nr: %d", record_id);
157 }
158
159 if (value_event->attribute_length <= attribute_value_buffer_size) {
160 attribute_value[value_event->data_offset] = value_event->data;
161
162 if ((uint16_t)(value_event->data_offset + 1) == value_event->attribute_length) {
163 //printf("SDP Attribute Data:");
164 //printf_hexdump(attribute_value, value_event->attribute_length);
165
166 switch (value_event->attribute_id) {
167 case SDP_ServiceClassIDList:
168 if (de_get_element_type(attribute_value) != DE_DES) break;
169 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) {
170 uint8_t * element = des_iterator_get_element(&des_list_it);
171 if (de_get_element_type(element) != DE_UUID) continue;
172 uint32_t uuid = de_get_uuid32(element);
173 switch (uuid) {
174 case BNEP_UUID_PANU:
175 case BNEP_UUID_NAP:
176 case BNEP_UUID_GN:
177 log_info("SDP Attribute 0x%04x: BNEP PAN protocol UUID: %04x", value_event->attribute_id, uuid);
178 sdp_bnep_remote_uuid = uuid;
179 break;
180 default:
181 log_info("SDP Attribute 0x%04x: UUID: %04x", value_event->attribute_id, uuid);
182 break;
183 }
184 }
185 break;
186 // 0x0100 "Service Name"
187 case 0x0100:
188 // 0x0101 "Service Description"
189 case 0x0101:
190 str = get_string_from_data_element(attribute_value);
191 log_info("SDP Attribute: 0x%04x: %s", value_event->attribute_id, str);
192 free(str);
193 break;
194 case SDP_ProtocolDescriptorList: {
195 log_info("SDP Attribute: 0x%04x", value_event->attribute_id);
196
197 for (des_iterator_init(&des_list_it, attribute_value); des_iterator_has_more(&des_list_it); des_iterator_next(&des_list_it)) {
198 uint8_t *des_element;
199 uint8_t *element;
200 uint32_t uuid;
201
202 if (des_iterator_get_type(&des_list_it) != DE_DES) continue;
203
204 des_element = des_iterator_get_element(&des_list_it);
205 des_iterator_init(&prot_it, des_element);
206 element = des_iterator_get_element(&prot_it);
207
208 if (de_get_element_type(element) != DE_UUID) continue;
209
210 uuid = de_get_uuid32(element);
211 switch (uuid) {
212 case SDP_L2CAPProtocol:
213 if (!des_iterator_has_more(&prot_it)) continue;
214 des_iterator_next(&prot_it);
215 de_element_get_uint16(des_iterator_get_element(&prot_it), &sdp_bnep_l2cap_psm);
216 break;
217 case SDP_BNEPProtocol:
218 if (!des_iterator_has_more(&prot_it)) continue;
219 des_iterator_next(&prot_it);
220 de_element_get_uint16(des_iterator_get_element(&prot_it), &sdp_bnep_version);
221 break;
222 default:
223 log_info("uuid 0x%04x", uuid);
224 break;
225 }
226 }
227 log_info("Summary: uuid 0x%04x, l2cap_psm 0x%04x, bnep_version 0x%04x", sdp_bnep_remote_uuid, sdp_bnep_l2cap_psm, sdp_bnep_version);
228 break;
229 }
230 default:
231 break;
232 }
233 }
234 }
235 else {
236 log_error("SDP attribute value buffer size exceeded: available %d, required %d", attribute_value_buffer_size, value_event->attribute_length);
237 }
238 break;
239
240 case SDP_QUERY_COMPLETE:
241 handle_sdp_client_record_complete();
242 complete_event = (sdp_query_complete_event_t*)event;
243 log_error("General query done with status %d, bnep psm %04x.", complete_event->status, bnep_l2cap_psm);
244 if (bnep_l2cap_psm) {
245 bnep_do_connect = complete_event->status == 0;
246 }
247 else {
248 log_error("No BNEP service found");
249 }
250 break;
251 }
252}
253
254static void packet_handler(void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
255{
256 uint8_t event;
257 bd_addr_t event_addr;
258 uint8_t rfcomm_channel_nr;
259 uint16_t uuid_source;
260 uint16_t uuid_dest;
261 uint16_t mtu;
262
263 switch (packet_type) {
264 case HCI_EVENT_PACKET:
265 event = packet[0];
266 switch (event) {
267 case BTSTACK_EVENT_STATE:
268 /* BT Stack activated, get started */
269 if (packet[2] == HCI_STATE_WORKING) {
270 hci_send_cmd(&hci_write_local_name, bt_localname);
271 /* Send a general query for BNEP Protocol ID */
272 log_info("Start SDP BNEP query.");
273 sdp_general_query_for_uuid(remote_addr, SDP_BNEPProtocol);
274 }
275 break;
276
277 case BTSTACK_EVENT_NR_CONNECTIONS_CHANGED:
278 if ((packet[2] == 0) && bnep_do_connect) {
279 bnep_do_connect = false;
280 /* Create BNEP connection */
281 bnep_connect(NULL, &remote_addr, bnep_l2cap_psm, bnep_remote_uuid);
282 }
283 break;
284
285 case HCI_EVENT_COMMAND_COMPLETE:
286 if (COMMAND_COMPLETE_EVENT(packet, hci_read_bd_addr)) {
287 bt_flip_addr(event_addr, &packet[6]);
288 log_info("BD-ADDR: %s", bd_addr_to_str(event_addr));
289
290 snprintf(bt_localname, sizeof(bt_localname), TARGET_NAME"%04x", ((event_addr[4] << 8) | event_addr[5]));
291 gap_set_local_name(bt_localname);
292 break;
293 }
294 if (COMMAND_COMPLETE_EVENT(packet, hci_write_local_name)) {
295 hci_discoverable_control(1);
296 break;
297 }
298 break;
299
300 case HCI_EVENT_LINK_KEY_REQUEST:
301 // deny link key request
302 log_info("Link key request");
303 bt_flip_addr(event_addr, &packet[2]);
304 hci_send_cmd(&hci_link_key_request_negative_reply, &event_addr);
305 break;
306
307 case HCI_EVENT_PIN_CODE_REQUEST:
308 // inform about pin code request
309 log_info("Pin code request - using '0000'");
310 bt_flip_addr(event_addr, &packet[2]);
311 hci_send_cmd(&hci_pin_code_request_reply, &event_addr, 4, "0000");
312 break;
313
314 case HCI_EVENT_USER_CONFIRMATION_REQUEST:
315 // inform about user confirmation request
316 log_info("HCI User Confirmation Request with numeric value '%06u'", READ_BT_32(packet, 8));
317 log_info("HCI User Confirmation Auto accept");
318 break;
319
320 case RFCOMM_EVENT_INCOMING_CONNECTION:
321 // data: event (8), len(8), address(48), channel (8), rfcomm_cid (16)
322 bt_flip_addr(event_addr, &packet[2]);
323 rfcomm_channel_nr = packet[8];
324 rfcomm_channel_id = READ_BT_16(packet, 9);
325 log_info("RFCOMM channel %u requested for %s", rfcomm_channel_nr, bd_addr_to_str(event_addr));
326 rfcomm_accept_connection_internal(rfcomm_channel_id);
327 break;
328
329 case RFCOMM_EVENT_OPEN_CHANNEL_COMPLETE:
330 // data: event(8), len(8), status (8), address (48), server channel(8), rfcomm_cid(16), max frame size(16)
331 if (packet[2]) {
332 log_info("RFCOMM channel open failed, status %u", packet[2]);
333 }
334 else {
335 rfcomm_channel_id = READ_BT_16(packet, 12);
336 mtu = READ_BT_16(packet, 14);
337 log_info("RFCOMM channel open succeeded. New RFCOMM Channel ID %u, max frame size %u", rfcomm_channel_id, mtu);
338 }
339 break;
340
341 case RFCOMM_EVENT_CHANNEL_CLOSED:
342 log_info("RFCOMM channel closed");
343 rfcomm_channel_id = 0;
344 break;
345
346 case BNEP_EVENT_OPEN_CHANNEL_COMPLETE:
347 if (packet[2]) {
348 log_info("BNEP channel open failed, status %02x", packet[2]);
349 }
350 else {
351 // data: event(8), len(8), status (8), bnep source uuid (16), bnep destination uuid (16), remote_address (48)
352 bnep_cid = READ_BT_16(packet, 3);
353 uuid_source = READ_BT_16(packet, 5);
354 uuid_dest = READ_BT_16(packet, 7);
355 mtu = READ_BT_16(packet, 9);
356 //bt_flip_addr(remote_addr, &packet[11]);
357 memcpy(&remote_addr, &packet[11], sizeof(bd_addr_t));
358 log_info("BNEP connection open succeeded to %s source UUID 0x%04x dest UUID: 0x%04x, max frame size %u", bd_addr_to_str(event_addr), uuid_source, uuid_dest, mtu);
359 /* Setup network interface */
360 btstack_network_up(bnep_cid, (uint8_t *)hci_local_bd_addr(), (uint8_t *)remote_addr);
361 log_info("Network Interface %s activated", btstack_network_get_name(bnep_cid));
362 }
363 break;
364
365 case BNEP_EVENT_CHANNEL_TIMEOUT:
366 log_info("BNEP channel timeout! Channel will be closed");
367 break;
368
369 case BNEP_EVENT_CHANNEL_CLOSED:
370 bnep_cid = READ_BT_16(packet, 2);
371 bt_channel_closed(bnep_cid);
372 break;
373
374 case BNEP_EVENT_READY_TO_SEND:
375 bnep_cid = READ_BT_16(packet, 2);
376 bt_send_packet(bnep_cid);
377 break;
378
379 default:
380 break;
381 }
382 break;
383
384 case RFCOMM_DATA_PACKET:
385 // loopback
386 if (rfcomm_channel_id) {
387 int err = rfcomm_send_internal(rfcomm_channel_id, packet, size);
388 if (err) {
389 log_error("rfcomm_send_internal -> error %d", err);
390 }
391 }
392 break;
393
394 case BNEP_DATA_PACKET:
395 bt_recv_packet(channel, packet, size);
396 break;
397
398 default:
399 break;
400 }
401}
402
403/*
404 * @section Network packet handler
405 *
406 * @text A pointer to the network packet is stored and a BNEP_EVENT_CAN_SEND_NOW requested
407 */
408
409 /* LISTING_START(networkPacketHandler): Network Packet Handler */
410static void network_send_packet_callback(uint16_t bnep_cid, const uint8_t * packet, uint16_t size) {
411 if ((bnep_cid == 0) || (network_buffer_len > 0)) {
412#ifdef PACKET_DUMP
413 int len = size;
414 printf("DROP %04x %d ", bnep_cid, size);
415 if (len > 128)
416 len = 128;
417 printf_hexdump(packet, len);
418#endif
419 if (network_buffer_len > 0) {
420 network_buffer_len = 0;
421 btstack_network_packet_sent(network_buffer_cid);
422 }
423 }
424
425 network_buffer_cid = bnep_cid;
426 network_buffer = packet;
427 network_buffer_len = size;
428}
429
430void panu_init(void)
431{
432 const hci_transport_t *transport;
433 bt_control_t * control;
434 hci_uart_config_t * config;
435 remote_device_db_t * remote_db;
436 service_record_item_t *spp_service_record_item;
437 service_record_item_t *bnep_service_record_item;
438
439 //hci_dump_open("", HCI_DUMP_PACKETLOGGER);
440
441 log_info("bt初期化...");
442
443 // start with BTstack init - especially configure HCI Transport
444 btstack_memory_init();
445 run_loop_init(RUN_LOOP_EMBEDDED);
446
447 /* Initialize HCI */
448 log_info("HCI初期化...");
449 transport = hci_transport_usb_instance();
450 control = NULL;
451 config = NULL;
452 remote_db = (remote_device_db_t *)&remote_device_db_memory;
453 hci_init(transport, config, control, remote_db);
454 hci_set_class_of_device(0x820300/*0x820810*/);
455
456 /* Initialize L2CAP */
457 log_info("L2CAP初期化...");
458 l2cap_init();
459 l2cap_register_packet_handler(packet_handler);
460
461 /* Initialise SDP, create record for SPP and register with SDP */
462 log_info("SDP初期化...");
463 sdp_init();
464
465 /* Initialize RFCOMM */
466 log_info("RFCOMM初期化...");
467 rfcomm_init();
468 rfcomm_register_packet_handler(packet_handler);
469 rfcomm_register_service_internal(NULL, RFCOMM_SERVER_CHANNEL, 0xffff); // reserved channel, mtu=100
470
471 /* Initialise BNEP */
472 log_info("BNEP初期化...");
473 bnep_init();
474 bnep_register_packet_handler(packet_handler);
475 bnep_register_service(NULL, BNEP_UUID_PANU, BNEP_MTU_MIN); /* Minimum L2CAP MTU for bnep is 1691 bytes */
476
477 memset(spp_service_buffer, 0, sizeof(spp_service_buffer));
478 memset(bnep_service_buffer, 0, sizeof(bnep_service_buffer));
479
480 // サービス定義
481 spp_service_record_item = (service_record_item_t *)spp_service_buffer;
482 bnep_service_record_item = (service_record_item_t *)bnep_service_buffer;
483
484 log_info("SPPサービス登録...");
485 sdp_create_spp_service((uint8_t*)&spp_service_record_item->service_record, 1, "RX SPP");
486 de_dump_data_element((uint8_t*)spp_service_record_item->service_record);
487 log_info("SSP SDP service buffer size: %u", (uint16_t)(sizeof(service_record_item_t) + de_get_len((uint8_t*)spp_service_record_item->service_record)));
488 sdp_register_service_internal(NULL, spp_service_record_item);
489
490 log_info("BNEPサービス登録...");
491 pan_create_panu_service((uint8_t*)&bnep_service_record_item->service_record, "RX PANU", "RX PANU", BNEP_SECURITY_NONE);
492 de_dump_data_element((uint8_t*)bnep_service_record_item->service_record);
493 log_info("PANU SDP service buffer size: %u", (uint16_t)(sizeof(service_record_item_t) + de_get_len((uint8_t*)bnep_service_record_item->service_record)));
494 sdp_register_service_internal(NULL, bnep_service_record_item);
495
496 // Secure Simple Pairing configuration -> just works
497 hci_ssp_set_enable(1);
498 hci_ssp_set_io_capability(SSP_IO_CAPABILITY_NO_INPUT_NO_OUTPUT);
499 hci_ssp_set_auto_accept(1);
500
501 /* Turn on the device */
502 log_info("HCI電源ON...");
503 hci_power_control(HCI_POWER_ON);
504
505 /* Initialise SDP */
506 sdp_parser_init();
507 sdp_parser_register_callback(handle_sdp_client_query_result);
508}
509
510void nap_init(void);
511
512void bt_init(void)
513{
514 btstack_network_init(&network_send_packet_callback);
515
516 if (bt_bnep_mode == 0) {
517 panu_init();
518 }
519 else {
520 nap_init();
521 }
522
523 bt_enabled = true;
524}
525
526void bt_task(void)
527{
528 extern void hal_tick_handler();
529
530 if (bt_enabled) {
531 hal_tick_handler();
532 run_loop_execute();
533 bt_send_packet(0xffff);
534 }
535}
536
537int bt_can_send_packet(void)
538{
539 return network_buffer_len == 0;
540}
541
542void bt_send_packet(uint16_t bnep_cid)
543{
544 if (network_buffer_len > 0) {
545 if (network_buffer_cid == 0) {
546 network_buffer_len = 0;
547 btstack_network_packet_sent(network_buffer_cid);
548 return;
549 }
550 /* Check for parked network packets and send it out now */
551 if (bnep_can_send_packet_now(network_buffer_cid)) {
552 uint16_t len = network_buffer_len;
553#ifdef PACKET_DUMP
554 printf("ETH %u => ", len);
555 if (len > 128)
556 len = 128;
557 printf_hexdump(network_buffer, len);
558#endif
559 // この関数はbnep_sendから再帰呼び出しされるので、
560 // bnep_sendを呼び出すより前に送信パケットなし状態にします。
561 network_buffer_len = 0;
562 bnep_send(network_buffer_cid, (uint8_t*)network_buffer, len);
563 btstack_network_packet_sent(network_buffer_cid);
564 }
565 }
566}
567
568void bt_recv_packet(uint16_t bnep_cid, const uint8_t * packet, uint16_t size)
569{
570#ifdef PACKET_DUMP
571 int len = size;
572 printf("ETH %d <= ", size);
573 if (len > 128)
574 len = 128;
575 printf_hexdump(packet, len);
576#endif
577 btstack_network_process_packet(bnep_cid, packet, size);
578}
579
580void bt_channel_closed(uint16_t bnep_cid)
581{
582 log_info("BNEP channel closed");
583 btstack_network_down(bnep_cid);
584 if ((network_buffer_len > 0) && (network_buffer_cid == bnep_cid)) {
585 network_buffer_len = 0;
586 btstack_network_packet_sent(network_buffer_cid);
587 }
588}
589
590void bt_terminate(void)
591{
592 hci_close();
593
594 bt_enabled = false;
595}
Note: See TracBrowser for help on using the repository browser.