xref: /btstack/src/classic/goep_server.c (revision 3a6b6ac96a1fa77eb45d4ddc3bb5cfaa40035674)
1 /*
2  * Copyright (C) 2019 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  * [email protected]
35  *
36  */
37 
38 #define BTSTACK_FILE__ "goep_server.c"
39 
40 #include "btstack_config.h"
41 
42 #include <stdint.h>
43 #include <string.h>
44 
45 #include "hci_cmd.h"
46 #include "btstack_debug.h"
47 #include "btstack_defines.h"
48 #include "hci.h"
49 #include "btstack_memory.h"
50 #include "hci_dump.h"
51 #include "btstack_event.h"
52 
53 #include "classic/goep_server.h"
54 #include "classic/obex_message_builder.h"
55 
56 #ifdef ENABLE_GOEP_L2CAP
57 #include "l2cap.h"
58 
59 static l2cap_ertm_config_t ertm_config = {
60     1,  // ertm mandatory
61     2,  // max transmit, some tests require > 1
62     2000,
63     12000,
64     (GOEP_SERVER_ERTM_BUFFER / 2),    // l2cap ertm mtu
65     2,
66     2,
67     1,      // 16-bit FCS
68 };
69 
70 static uint8_t goep_server_l2cap_packet_buffer[1000];
71 
72 #endif
73 
74 static btstack_linked_list_t goep_server_connections = NULL;
75 static btstack_linked_list_t goep_server_services = NULL;
76 static uint16_t goep_server_cid_counter = 0;
77 
78 static goep_server_service_t * goep_server_get_service_for_rfcomm_channel(uint8_t rfcomm_channel){
79     btstack_linked_item_t *it;
80     for (it = (btstack_linked_item_t *) goep_server_services; it ; it = it->next){
81         goep_server_service_t * service = ((goep_server_service_t *) it);
82         if (service->rfcomm_channel == rfcomm_channel){
83             return service;
84         };
85     }
86     return NULL;
87 }
88 
89 #ifdef ENABLE_GOEP_L2CAP
90 static goep_server_service_t * goep_server_get_service_for_l2cap_psm(uint16_t l2cap_psm){
91     btstack_linked_item_t *it;
92     for (it = (btstack_linked_item_t *) goep_server_services; it ; it = it->next){
93         goep_server_service_t * service = ((goep_server_service_t *) it);
94         if (service->l2cap_psm == l2cap_psm){
95             return service;
96         };
97     }
98     return NULL;
99 }
100 #endif
101 
102 static goep_server_connection_t * goep_server_get_connection_for_rfcomm_cid(uint16_t bearer_cid){
103     btstack_linked_item_t *it;
104     for (it = (btstack_linked_item_t *) goep_server_connections; it ; it = it->next){
105         goep_server_connection_t * connection = ((goep_server_connection_t *) it);
106         if (connection->type != GOEP_CONNECTION_RFCOMM) continue;
107         if (connection->bearer_cid == bearer_cid){
108             return connection;
109         };
110     }
111     return NULL;
112 }
113 
114 #ifdef ENABLE_GOEP_L2CAP
115 static goep_server_connection_t * goep_server_get_connection_for_l2cap_cid(uint16_t bearer_cid){
116     btstack_linked_item_t *it;
117     for (it = (btstack_linked_item_t *) goep_server_connections; it ; it = it->next){
118         goep_server_connection_t * connection = ((goep_server_connection_t *) it);
119         if (connection->type != GOEP_CONNECTION_L2CAP) continue;
120         if (connection->bearer_cid == bearer_cid){
121             return connection;
122         };
123     }
124     return NULL;
125 }
126 #endif
127 
128 static goep_server_connection_t * goep_server_get_connection_for_goep_cid(uint16_t goep_cid){
129     btstack_linked_item_t *it;
130     for (it = (btstack_linked_item_t *) goep_server_connections; it ; it = it->next){
131         goep_server_connection_t * connection = ((goep_server_connection_t *) it);
132         if (connection->goep_cid == goep_cid){
133             return connection;
134         };
135     }
136     return NULL;
137 }
138 
139 static uint16_t goep_server_get_next_goep_cid(void){
140     goep_server_cid_counter++;
141     if (goep_server_cid_counter == 0){
142         goep_server_cid_counter = 1;
143     }
144     return goep_server_cid_counter;
145 }
146 
147 static inline void goep_server_emit_incoming_connection(btstack_packet_handler_t callback, uint16_t goep_cid, bd_addr_t bd_addr, hci_con_handle_t con_handle){
148     uint8_t event[13];
149     uint16_t pos = 0;
150     event[pos++] = HCI_EVENT_GOEP_META;
151     event[pos++] = 15 - 2;
152     event[pos++] = GOEP_SUBEVENT_INCOMING_CONNECTION;
153     little_endian_store_16(event, pos, goep_cid);
154     pos+=2;
155     memcpy(&event[pos], bd_addr, 6);
156     pos += 6;
157     little_endian_store_16(event, pos, con_handle);
158     pos += 2;
159     btstack_assert(pos == sizeof(event));
160     callback(HCI_EVENT_PACKET, goep_cid, &event[0], pos);
161 }
162 
163 static inline void goep_server_emit_connection_opened(btstack_packet_handler_t callback, uint16_t goep_cid, bd_addr_t bd_addr, hci_con_handle_t con_handle, uint8_t status){
164     uint8_t event[15];
165     uint16_t pos = 0;
166     event[pos++] = HCI_EVENT_GOEP_META;
167     event[pos++] = 15 - 2;
168     event[pos++] = GOEP_SUBEVENT_CONNECTION_OPENED;
169     little_endian_store_16(event, pos, goep_cid);
170     pos+=2;
171     event[pos++] = status;
172     memcpy(&event[pos], bd_addr, 6);
173     pos += 6;
174     little_endian_store_16(event, pos, con_handle);
175     pos += 2;
176     event[pos++] = 1;
177     btstack_assert(pos == sizeof(event));
178     callback(HCI_EVENT_PACKET, goep_cid, &event[0], pos);
179 }
180 
181 static inline void goep_server_emit_connection_closed(btstack_packet_handler_t callback, uint16_t goep_cid){
182     uint8_t event[5];
183     uint16_t pos = 0;
184     event[pos++] = HCI_EVENT_GOEP_META;
185     event[pos++] = 5 - 3;
186     event[pos++] = GOEP_SUBEVENT_CONNECTION_CLOSED;
187     little_endian_store_16(event, pos, goep_cid);
188     pos += 2;
189     btstack_assert(pos == sizeof(event));
190     callback(HCI_EVENT_PACKET, goep_cid, &event[0], pos);
191 }
192 
193 static inline void goep_server_emit_can_send_now_event(goep_server_connection_t * connection){
194     uint8_t event[5];
195     uint16_t pos = 0;
196     event[pos++] = HCI_EVENT_GOEP_META;
197     event[pos++] = 5 - 3;
198     event[pos++] = GOEP_SUBEVENT_CAN_SEND_NOW;
199     little_endian_store_16(event,pos,connection->goep_cid);
200     pos += 2;
201     btstack_assert(pos == sizeof(event));
202     connection->callback(HCI_EVENT_PACKET, connection->goep_cid, &event[0], pos);
203 }
204 
205 static void goep_server_handle_connection_opened(goep_server_connection_t * context, bd_addr_t addr, hci_con_handle_t con_handle, uint8_t status, uint16_t bearer_cid, uint16_t bearer_mtu){
206 
207     uint16_t goep_cid = context->goep_cid;
208     btstack_packet_handler_t packet_handler = context->callback;
209 
210     if (status) {
211         log_info("goep_client: open failed, status %u", status);
212         btstack_linked_list_remove(&goep_server_connections, (btstack_linked_item_t *) context);
213         btstack_memory_goep_server_connection_free(context);
214     } else {
215         // context->bearer_mtu = mtu;
216         context->state = GOEP_SERVER_CONNECTED;
217         context->bearer_cid = bearer_cid;
218 #ifdef ENABLE_GOEP_L2CAP
219         if (context->type == GOEP_CONNECTION_L2CAP){
220             bearer_mtu = btstack_min(bearer_mtu, sizeof(goep_server_l2cap_packet_buffer));
221         }
222 #endif
223         context->bearer_mtu = bearer_mtu;
224         log_info("goep_server: connection opened. cid %u, max frame size %u", context->bearer_cid, bearer_mtu);
225     }
226 
227     goep_server_emit_connection_opened(packet_handler, goep_cid, addr, con_handle, status);
228 }
229 
230 static void goep_server_handle_connection_closed(goep_server_connection_t * goep_connection){
231     uint16_t goep_cid = goep_connection->goep_cid;
232     btstack_packet_handler_t packet_handler = goep_connection->callback;
233 
234     btstack_linked_list_remove(&goep_server_connections, (btstack_linked_item_t *) goep_connection);
235     btstack_memory_goep_server_connection_free(goep_connection);
236 
237     goep_server_emit_connection_closed(packet_handler, goep_cid);
238 }
239 
240 #ifdef ENABLE_GOEP_L2CAP
241 static void goep_server_packet_handler_l2cap(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
242     UNUSED(channel);
243     UNUSED(size);
244 
245     bd_addr_t event_addr;
246     uint16_t l2cap_psm;
247     uint16_t l2cap_cid;
248     goep_server_connection_t * goep_connection;
249     goep_server_service_t *    goep_service;
250 
251     switch (packet_type){
252         case HCI_EVENT_PACKET:
253             switch (hci_event_packet_get_type(packet)) {
254                 case L2CAP_EVENT_INCOMING_CONNECTION:
255                     l2cap_psm = l2cap_event_incoming_connection_get_psm(packet);
256                     l2cap_cid  = l2cap_event_incoming_connection_get_local_cid(packet);
257                     goep_service = goep_server_get_service_for_l2cap_psm(l2cap_psm);
258                     if (!goep_service){
259                         l2cap_decline_connection(l2cap_cid);
260                         break;
261                     }
262 
263                     // alloc structure
264                     goep_connection = btstack_memory_goep_server_connection_get();
265                     if (!goep_connection){
266                         l2cap_decline_connection(l2cap_cid);
267                         break;
268                     }
269 
270                     // setup connection
271                     goep_connection->goep_cid   = goep_server_get_next_goep_cid();
272                     goep_connection->bearer_cid = l2cap_cid;
273                     goep_connection->callback   = goep_service->callback;
274                     goep_connection->type       = GOEP_CONNECTION_L2CAP;
275                     goep_connection->state      = GOEP_SERVER_W4_ACCEPT_REJECT;
276                     btstack_linked_list_add(&goep_server_connections, (btstack_linked_item_t *) goep_connection);
277 
278                     // notify user
279                     l2cap_event_incoming_connection_get_address(packet, event_addr);
280                     goep_server_emit_incoming_connection(goep_service->callback, goep_connection->goep_cid, event_addr,
281                                                          l2cap_event_incoming_connection_get_handle(packet));
282                     break;
283 
284                 case L2CAP_EVENT_CHANNEL_OPENED:
285                     l2cap_cid = l2cap_event_channel_opened_get_local_cid(packet);
286                     goep_connection = goep_server_get_connection_for_l2cap_cid(l2cap_cid);
287                     btstack_assert(goep_connection != NULL);
288                     btstack_assert(goep_connection->state == GOEP_SERVER_W4_CONNECTED);
289                     l2cap_event_channel_opened_get_address(packet, event_addr);
290                     goep_server_handle_connection_opened(goep_connection, event_addr,
291                                                          l2cap_event_channel_opened_get_handle(packet),
292                                                          l2cap_event_channel_opened_get_status(packet),
293                                                          l2cap_cid,
294                                                          l2cap_event_channel_opened_get_remote_mtu(packet) );
295                     return;
296                 case L2CAP_EVENT_CAN_SEND_NOW:
297                     l2cap_cid = l2cap_event_can_send_now_get_local_cid(packet);
298                     goep_connection = goep_server_get_connection_for_l2cap_cid(l2cap_cid);
299                     btstack_assert(goep_connection != NULL);
300                     goep_server_emit_can_send_now_event(goep_connection);
301                     break;
302                 case L2CAP_EVENT_CHANNEL_CLOSED:
303                     l2cap_cid = l2cap_event_channel_closed_get_local_cid(packet);
304                     goep_connection = goep_server_get_connection_for_l2cap_cid(l2cap_cid);
305                     btstack_assert(goep_connection != NULL);
306                     goep_server_handle_connection_closed(goep_connection);
307                     break;
308                 default:
309                     break;
310             }
311             break;
312         case L2CAP_DATA_PACKET:
313             goep_connection = goep_server_get_connection_for_l2cap_cid(channel);
314             btstack_assert(goep_connection != NULL);
315             goep_connection->callback(GOEP_DATA_PACKET, goep_connection->goep_cid, packet, size);
316             break;
317         default:
318             break;
319     }
320 }
321 #endif
322 
323 static void goep_server_packet_handler_rfcomm(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){
324     UNUSED(channel);
325     UNUSED(size);
326 
327     bd_addr_t event_addr;
328     uint8_t  rfcomm_channel;
329     uint16_t rfcomm_cid;
330     goep_server_service_t    * goep_service;
331     goep_server_connection_t * goep_connection;
332 
333     switch (packet_type){
334         case HCI_EVENT_PACKET:
335             switch (hci_event_packet_get_type(packet)) {
336                 case RFCOMM_EVENT_INCOMING_CONNECTION:
337                     rfcomm_channel = rfcomm_event_incoming_connection_get_server_channel(packet);
338                     rfcomm_cid = rfcomm_event_incoming_connection_get_rfcomm_cid(packet);
339 
340                     goep_service = goep_server_get_service_for_rfcomm_channel(rfcomm_channel);
341                     if (!goep_service){
342                         rfcomm_decline_connection(rfcomm_cid);
343                         break;
344                     }
345 
346                     // alloc structure
347                     goep_connection = btstack_memory_goep_server_connection_get();
348                     if (!goep_connection){
349                         rfcomm_decline_connection(rfcomm_cid);
350                         break;
351                     }
352 
353                     // setup connection
354                     goep_connection->goep_cid   = goep_server_get_next_goep_cid();
355                     goep_connection->bearer_cid = rfcomm_cid;
356                     goep_connection->callback   = goep_service->callback;
357                     goep_connection->type       = GOEP_CONNECTION_RFCOMM;
358                     goep_connection->state      = GOEP_SERVER_W4_ACCEPT_REJECT;
359                     btstack_linked_list_add(&goep_server_connections, (btstack_linked_item_t *) goep_connection);
360 
361                     // notify user
362                     rfcomm_event_incoming_connection_get_bd_addr(packet, event_addr);
363                     goep_server_emit_incoming_connection(goep_service->callback, goep_connection->goep_cid, event_addr,
364                                                          rfcomm_event_incoming_connection_get_con_handle(packet));
365                     break;
366 
367                 case RFCOMM_EVENT_CHANNEL_OPENED:
368                     rfcomm_cid = rfcomm_event_channel_opened_get_rfcomm_cid(packet);
369                     goep_connection = goep_server_get_connection_for_rfcomm_cid(rfcomm_cid);
370                     btstack_assert(goep_connection != NULL);
371                     btstack_assert(goep_connection->state == GOEP_SERVER_W4_CONNECTED);
372                     rfcomm_event_channel_opened_get_bd_addr(packet, event_addr);
373                     goep_server_handle_connection_opened(goep_connection, event_addr,
374                                                          rfcomm_event_channel_opened_get_con_handle(packet),
375                                                          rfcomm_event_channel_opened_get_status(packet),
376                                                          rfcomm_cid,
377                                                          rfcomm_event_channel_opened_get_max_frame_size(packet) );
378                     break;
379 
380                 case RFCOMM_EVENT_CAN_SEND_NOW:
381                     rfcomm_cid = rfcomm_event_can_send_now_get_rfcomm_cid(packet);
382                     goep_connection = goep_server_get_connection_for_rfcomm_cid(rfcomm_cid);
383                     btstack_assert(goep_connection != NULL);
384                     goep_server_emit_can_send_now_event(goep_connection);
385                     break;
386 
387                 case RFCOMM_EVENT_CHANNEL_CLOSED:
388                     rfcomm_cid = rfcomm_event_channel_closed_get_rfcomm_cid(packet);
389                     goep_connection = goep_server_get_connection_for_rfcomm_cid(rfcomm_cid);
390                     btstack_assert(goep_connection != NULL);
391                     goep_server_handle_connection_closed(goep_connection);
392                     break;
393 
394                 default:
395                     break;
396             }
397             break;
398 
399         case RFCOMM_DATA_PACKET:
400             goep_connection = goep_server_get_connection_for_rfcomm_cid(channel);
401             btstack_assert(goep_connection != NULL);
402             goep_connection->callback(GOEP_DATA_PACKET, goep_connection->goep_cid, packet, size);
403             break;
404 
405         default:
406             break;
407     }
408 }
409 
410 void goep_server_init(void){
411 }
412 
413 uint8_t goep_server_register_service(btstack_packet_handler_t callback, uint8_t rfcomm_channel, uint16_t rfcomm_max_frame_size,
414                 uint16_t l2cap_psm, uint16_t l2cap_mtu, gap_security_level_t security_level){
415 
416     log_info("rfcomm_channel 0x%02x rfcomm_max_frame_size %u l2cap_psm 0x%02x l2cap_mtu %u",
417              rfcomm_channel, rfcomm_max_frame_size, l2cap_psm, l2cap_mtu);
418 
419     // check if service is already registered
420     goep_server_service_t * service;
421     service = goep_server_get_service_for_rfcomm_channel(rfcomm_channel);
422     if (service != NULL) {
423         return RFCOMM_CHANNEL_ALREADY_REGISTERED;
424     }
425 
426 #ifdef ENABLE_GOEP_L2CAP
427     if (l2cap_psm != 0){
428         service = goep_server_get_service_for_l2cap_psm(l2cap_psm);
429         if (service != NULL) {
430             return L2CAP_SERVICE_ALREADY_REGISTERED;
431         }
432     }
433 #endif
434 
435     // alloc structure
436     service = btstack_memory_goep_server_service_get();
437     if (service == NULL) {
438         return BTSTACK_MEMORY_ALLOC_FAILED;
439     }
440 
441     // fill in
442     service->callback = callback;
443     service->rfcomm_channel = rfcomm_channel;
444     service->l2cap_psm = l2cap_psm;
445 
446     uint8_t status = ERROR_CODE_SUCCESS;
447     bool rfcomm_registered = false;
448 
449 #ifdef ENABLE_GOEP_L2CAP
450     bool l2cap_registered = false;
451     // register with L2CAP
452     if (l2cap_psm != 0){
453         status = l2cap_register_service(goep_server_packet_handler_l2cap, l2cap_psm, l2cap_mtu, security_level);
454         if (status == ERROR_CODE_SUCCESS){
455             l2cap_registered = true;
456         }
457     }
458 #endif
459 
460     // register with RFCOMM
461     if (status == ERROR_CODE_SUCCESS){
462         status = rfcomm_register_service(goep_server_packet_handler_rfcomm, rfcomm_channel, rfcomm_max_frame_size);
463         if (status == ERROR_CODE_SUCCESS){
464             rfcomm_registered = true;
465         }
466     }
467 
468     // add service on success
469     if (status == ERROR_CODE_SUCCESS){
470         btstack_linked_list_add(&goep_server_services, (btstack_linked_item_t *) service);
471         return ERROR_CODE_SUCCESS;
472     }
473 
474     // unrestore otherwise
475     btstack_memory_goep_server_service_free(service);
476 #ifdef ENABLE_GOEP_L2CAP
477     if (l2cap_registered){
478         l2cap_unregister_service(l2cap_psm);
479     }
480 #endif
481     if (rfcomm_registered){
482         rfcomm_unregister_service(rfcomm_channel);
483     }
484     return status;
485 }
486 
487 uint8_t goep_server_accept_connection(uint16_t goep_cid){
488     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
489     if (connection == NULL){
490         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
491     }
492     if (connection->state != GOEP_SERVER_W4_ACCEPT_REJECT){
493         return ERROR_CODE_COMMAND_DISALLOWED;
494     }
495     connection->state = GOEP_SERVER_W4_CONNECTED;
496 #ifdef ENABLE_GOEP_L2CAP
497     if (connection->type == GOEP_CONNECTION_L2CAP){
498         return l2cap_ertm_accept_connection(connection->bearer_cid, &ertm_config, connection->ertm_buffer, GOEP_SERVER_ERTM_BUFFER);
499     }
500 #endif
501     return rfcomm_accept_connection(connection->bearer_cid);
502 }
503 
504 uint8_t goep_server_decline_connection(uint16_t goep_cid){
505     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
506     if (connection == NULL){
507         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
508     }
509     connection->state = GOEP_SERVER_W4_CONNECTED;
510     if (connection->state != GOEP_SERVER_W4_ACCEPT_REJECT){
511         return ERROR_CODE_COMMAND_DISALLOWED;
512     }
513 #ifdef ENABLE_GOEP_L2CAP
514     if (connection->type == GOEP_CONNECTION_L2CAP){
515         l2cap_decline_connection(connection->bearer_cid);
516         return ERROR_CODE_SUCCESS;
517     }
518 #endif
519     return rfcomm_decline_connection(connection->bearer_cid);
520 }
521 
522 uint8_t goep_server_request_can_send_now(uint16_t goep_cid){
523     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
524     if (connection == NULL){
525         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
526     }
527 
528     switch (connection->type){
529         case GOEP_CONNECTION_RFCOMM:
530             rfcomm_request_can_send_now_event(connection->bearer_cid);
531             break;
532 #ifdef ENABLE_GOEP_L2CAP
533         case GOEP_CONNECTION_L2CAP:
534             l2cap_request_can_send_now_event(connection->bearer_cid);
535             break;
536 #endif
537         default:
538             btstack_unreachable();
539             break;
540     }
541     return ERROR_CODE_SUCCESS;
542 }
543 
544 static uint8_t * goep_server_get_outgoing_buffer(goep_server_connection_t * connection){
545     switch (connection->type){
546 #ifdef ENABLE_GOEP_L2CAP
547         case GOEP_CONNECTION_L2CAP:
548             return goep_server_l2cap_packet_buffer;
549 #endif
550         case GOEP_CONNECTION_RFCOMM:
551             return rfcomm_get_outgoing_buffer();
552         default:
553             btstack_unreachable();
554             return NULL;
555     }
556 }
557 
558 static uint16_t goep_server_get_outgoing_buffer_len(goep_server_connection_t * connection){
559     return connection->bearer_mtu;
560 }
561 
562 static void goep_server_packet_init(goep_server_connection_t * connection){
563     switch (connection->type){
564 #ifdef ENABLE_GOEP_L2CAP
565         case GOEP_CONNECTION_L2CAP:
566             break;
567 #endif
568         case GOEP_CONNECTION_RFCOMM:
569             rfcomm_reserve_packet_buffer();
570             break;
571         default:
572             btstack_unreachable();
573             break;
574     }
575 }
576 
577 uint8_t goep_server_response_create_connect(uint16_t goep_cid, uint8_t obex_version_number, uint8_t flags, uint16_t maximum_obex_packet_length){
578     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
579     if (connection == NULL) {
580         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
581     }
582 
583     goep_server_packet_init(connection);
584 
585     // workaround: limit OBEX packet len to L2CAP/RFCOMM MTU
586     maximum_obex_packet_length = btstack_min(maximum_obex_packet_length, connection->bearer_mtu);
587 
588     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
589     uint16_t buffer_len = goep_server_get_outgoing_buffer_len(connection);
590     return obex_message_builder_response_create_connect(buffer, buffer_len, obex_version_number, flags, maximum_obex_packet_length, (uint32_t) goep_cid);
591 }
592 
593 uint8_t goep_server_response_create_general(uint16_t goep_cid, uint8_t opcode){
594     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
595     if (connection == NULL) {
596         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
597     }
598     goep_server_packet_init(connection);
599     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
600     uint16_t buffer_len = goep_server_get_outgoing_buffer_len(connection);
601     return obex_message_builder_response_create_general(buffer, buffer_len, opcode);
602 }
603 
604 uint8_t goep_server_header_add_end_of_body(uint16_t goep_cid, const uint8_t * end_of_body, uint16_t length){
605     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
606     if (connection == NULL) {
607         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
608     }
609 
610     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
611     uint16_t buffer_len = goep_server_get_outgoing_buffer_len(connection);
612     return obex_message_builder_body_add_static(buffer, buffer_len, end_of_body, length);
613 }
614 
615 uint8_t goep_server_header_add_who(uint16_t goep_cid, const uint8_t * target){
616     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
617     if (connection == NULL) {
618         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
619     }
620 
621     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
622     uint16_t buffer_len = goep_server_get_outgoing_buffer_len(connection);
623     return obex_message_builder_header_add_who(buffer, buffer_len, target);
624 }
625 
626 uint8_t goep_server_header_add_srm_enable(uint16_t goep_cid){
627     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
628     if (connection == NULL) {
629         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
630     }
631 
632     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
633     uint16_t buffer_len = goep_server_get_outgoing_buffer_len(connection);
634     return obex_message_builder_header_add_srm_enable(buffer, buffer_len);
635 }
636 
637 uint8_t goep_server_execute(uint16_t goep_cid){
638     goep_server_connection_t * connection = goep_server_get_connection_for_goep_cid(goep_cid);
639     if (connection == NULL) {
640         return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER;
641     }
642 
643     uint8_t * buffer = goep_server_get_outgoing_buffer(connection);
644     uint16_t pos = big_endian_read_16(buffer, 1);
645     switch (connection->type) {
646 #ifdef ENABLE_GOEP_L2CAP
647         case GOEP_CONNECTION_L2CAP:
648             return l2cap_send(connection->bearer_cid, buffer, pos);
649             break;
650 #endif
651         case GOEP_CONNECTION_RFCOMM:
652             return rfcomm_send_prepared(connection->bearer_cid, pos);
653             break;
654         default:
655             btstack_unreachable();
656             return ERROR_CODE_SUCCESS;
657     }
658 }
659 
660 void goep_server_deinit(void){
661     goep_server_cid_counter = 0;
662     goep_server_services = NULL;
663 }
664