1 /*
2  * MBDyn (C) is a multibody analysis code.
3  * http://www.mbdyn.org
4  *
5  * Copyright (C) 1996-2005
6  *
7  * Pierangelo Masarati	<masarati@aero.polimi.it>
8  * Paolo Mantegazza	<mantegazza@aero.polimi.it>
9  *
10  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
11  * via La Masa, 34 - 20156 Milano, Italy
12  * http://www.aero.polimi.it
13  *
14  * Changing this copyright notice is forbidden.
15  *
16  * This program is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation (version 2 of the License).
19  *
20  *
21  * This program is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this program; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
29  */
30 
31 /*
32  * COPYRIGHT (C) 2003-2004
33  *
34  * Michele Attolico <attolico@aero.polimi.it>
35  *
36  * This library is free software; you can redistribute it and/or
37  * modify it under the terms of the GNU Lesser General Public
38  * License as published by the Free Software Foundation; either
39  * version 2 of the License, or (at your option) any later version.
40  *
41  * This library is distributed in the hope that it will be useful,
42  * but WITHOUT ANY WARRANTY; without even the implied warranty of
43  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
44  * Lesser General Public License for more details.
45  *
46  * You should have received a copy of the GNU Lesser General Public
47  * License along with this library; if not, write to the Free Software
48  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
49  */
50 
51 #undef VERBOSE
52 
53 #define S_FUNCTION_NAME		sfun_mbdyn_com_write
54 #define S_FUNCTION_LEVEL	2
55 
56 #ifdef MATLAB_MEX_FILE
57 #include "mex.h"
58 #endif
59 #include "simstruc.h"
60 
61 #define HOST_NAME_PARAM		ssGetSFcnParam(S, 0)
62 #define MBD_NAME_PARAM		ssGetSFcnParam(S, 1)
63 #define MBX_NAME_PARAM		ssGetSFcnParam(S, 2)
64 #define MBX_N_CHN_PARAM		ssGetSFcnParam(S, 3)
65 #define SAMPLE_TIME_PARAM	ssGetSFcnParam(S, 4)
66 
67 #define NET_PARAM		ssGetSFcnParam(S, 5)
68 #define PORT_PARAM		ssGetSFcnParam(S, 6)
69 #define PATH_PARAM		ssGetSFcnParam(S, 7)
70 
71 #define NUMBER_OF_PARAMS	(8)
72 
73 #define WAIT_LOOP  (10000)
74 
75 #define MBX_N_CHN		((uint_T) mxGetPr(MBX_N_CHN_PARAM)[0])
76 #define SAMPLE_TIME		((real_T) mxGetPr(SAMPLE_TIME_PARAM)[0])
77 
78 #define NET			((uint_T) mxGetPr(NET_PARAM)[0])
79 #define PORT			((uint_T) mxGetPr(PORT_PARAM)[0])
80 
81 #define NANO_SAMPLE_TIME	((long int)(SAMPLE_TIME*1000000000))
82 
83 #include <errno.h>
84 #include <stdio.h>
85 #include <unistd.h>
86 #include <sys/types.h>
87 #include <sys/mman.h>
88 #include <sys/stat.h>
89 #include <fcntl.h>
90 #include <sys/param.h>
91 #include <netdb.h>
92 #include <netinet/in.h>
93 #include <arpa/inet.h>
94 
95 #ifndef MATLAB_MEX_FILE
96 
97 #define KEEP_STATIC_INLINE
98 #include <math.h>
99 #include <rtai_lxrt.h>
100 #include <rtai_mbx.h>
101 #include <rtai_netrpc.h>
102 #include "mbdyn_rtai.h"
103 
104 bool          MBDynTaskActive[MAX_MBDYN_TASK]={false};
105 long int 	MBDynNode[MAX_MBDYN_TASK]={1};
106 unsigned long MBDynName[MAX_MBDYN_TASK]={0xFFFFFFFF};
107 
108 RT_TASK *rt_HostInterfaceTask;
109 
110 extern long int 	MBDynNode[];
111 extern unsigned long	MBDynName[];
112 extern bool		MBDynTaskActive[];
113 
114 static char		msg = 't';
115 extern RT_TASK		*rt_HostInterfaceTask;
116 struct mbd_t {
117 	void		*comptr;
118 	unsigned long	node;
119 	int		port;
120 	int		mbdtask_count;
121 };
122 
123 #else
124 
125 #include <string.h>
126 #include <stdlib.h>
127 #include <sys/socket.h>
128 #include <sys/un.h>
129 #include <sys/poll.h>
130 
131 #define TIME_OUT	10000	/* milliseconds */
132 
133 #endif /* MATLAB_MEX_FILE */
134 
135 #define MDL_CHECK_PARAMETERS
136 #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
137 static void
mdlCheckParameters(SimStruct * S)138 mdlCheckParameters(SimStruct *S)
139 {
140 	static char_T errMsg[BUFSIZ];
141 
142 	if (mxGetNumberOfElements(MBX_N_CHN_PARAM) != 1) {
143 		snprintf(errMsg, sizeof(errMsg),
144 			"Channel parameter must be a scalar.\n");
145 		ssSetErrorStatus(S, errMsg);
146 	}
147 
148 	if (mxGetNumberOfElements(SAMPLE_TIME_PARAM) != 1) {
149 		snprintf(errMsg, sizeof(errMsg),
150 			"Sample time parameter must be a scalar\n");
151 		ssSetErrorStatus(S, errMsg);
152 	}
153 
154 	if (mxGetNumberOfElements(PORT_PARAM) != 1) {
155 		snprintf(errMsg, sizeof(errMsg),
156 			"Port parameter must be a scalar\n");
157 		ssSetErrorStatus(S, errMsg);
158 	}
159 
160 	if (NET) {
161 		if (PORT == 0) {
162 			snprintf(errMsg, sizeof(errMsg),
163 				"Port must be defined\n");
164 			ssSetErrorStatus(S, errMsg);
165 		}
166 	}
167 }
168 #endif /* MDL_CHECK_PARAMETERS && MATLAB_MEX_FILE */
169 
170 static void
mdlInitializeSizes(SimStruct * S)171 mdlInitializeSizes(SimStruct *S)
172 {
173 	uint_T i;
174 
175 	ssSetNumSFcnParams(S, NUMBER_OF_PARAMS);
176 #if defined(MATLAB_MEX_FILE)
177 	if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
178 		mdlCheckParameters(S);
179 		if (ssGetErrorStatus(S) != NULL) {
180 			return;
181 		}
182 
183 	} else {
184 		return;
185 	}
186 #endif /* MATLAB_MEX_FILE */
187 
188 	for (i = 0; i < NUMBER_OF_PARAMS; i++) {
189 		ssSetSFcnParamNotTunable(S, i);
190 	}
191 	ssSetNumInputPorts(S, MBX_N_CHN);
192 	ssSetNumOutputPorts(S, MBX_N_CHN);
193 	for (i = 0; i < MBX_N_CHN; i++) {
194 		ssSetOutputPortWidth(S, i, 1);
195 		ssSetInputPortWidth(S, i, 1);
196 #ifdef MATLAB_MEX_FILE
197 		ssSetInputPortRequiredContiguous(S, i, 1);
198     		ssSetInputPortDirectFeedThrough(S, i, 1);
199 #endif /*MATLAB_MEX_FILE*/
200 	}
201 
202 	ssSetNumContStates(S, 0);
203 	ssSetNumDiscStates(S, 0);
204 	ssSetNumSampleTimes(S, 1);
205 	ssSetNumPWork(S, 1);
206 
207 #ifdef MATLAB_MEX_FILE
208 	ssSetNumIWork(S, 2);
209 #endif /* MATLAB_MEX_FILE */
210 }
211 
212 static void
mdlInitializeSampleTimes(SimStruct * S)213 mdlInitializeSampleTimes(SimStruct *S)
214 {
215 	ssSetSampleTime(S, 0, SAMPLE_TIME);
216 	ssSetOffsetTime(S, 0, 0.0);
217 }
218 
219 #define MDL_START
220 #if defined(MDL_START)
221 static void
mdlStart(SimStruct * S)222 mdlStart(SimStruct *S)
223 {
224 #ifndef MATLAB_MEX_FILE
225 	struct mbd_t		*ptrstr = NULL;
226 	static char_T		errMsg[BUFSIZ];
227 	register int		i;
228 	char			mbx_name[7], mbd_name[7];
229 	struct in_addr		addr;
230 
231 	char			host_name[MAXHOSTNAMELEN];
232 	void			*mbdtask = NULL;
233 	int			timerflag = 0, count = 0;
234 
235 	mxGetString(MBD_NAME_PARAM, mbd_name, 7);
236 	mxGetString(MBX_NAME_PARAM, mbx_name, 7);
237 
238 	/******************************************************************
239 	 * alloc struct
240 	 ******************************************************************/
241 
242 	ptrstr = (struct mbd_t *)malloc(sizeof(struct mbd_t));
243 	if (!ptrstr) {
244 		snprintf(errMsg, sizeof(errMsg),
245 			"\ncannot alloc memory for struct mbd_t\n");
246 		ssSetErrorStatus(S, errMsg);
247 		printf("%s", errMsg);
248 	}
249 	ssGetPWork(S)[0] = ptrstr;
250 
251 	/******************************************************************
252 	 * converting host_name into node
253 	 ******************************************************************/
254 	mxGetString(HOST_NAME_PARAM, host_name, sizeof(host_name));
255 
256 	inet_aton(host_name, &addr);
257 	ptrstr->node = addr.s_addr;
258 	if (ptrstr->node) {
259 		ptrstr->port = rt_request_hard_port(ptrstr->node);
260 		if (!ptrstr->port) {
261 			snprintf(errMsg, sizeof(errMsg),
262 				"rt_request_hard_port(%s) failed\n",
263 				ptrstr->node );
264 			ssSetErrorStatus(S, errMsg);
265 			printf("%s", errMsg);
266 			return;
267 		}
268 
269 	} else {
270 		ptrstr->port = 0;
271 	}
272 
273 	/* start real-time timer */
274 	if (!rt_is_hard_timer_running()) {
275 		rt_set_oneshot_mode();
276 		start_rt_timer(0);
277 		timerflag = 1;
278 	}
279 
280 	/******************************************************************
281 	 * find mbdyn task
282 	 ******************************************************************/
283 	count = 0;
284 	printf("\nwrite: MAX_MBDYN_TASK = %d \n", MAX_MBDYN_TASK);
285 	while (count < MAX_MBDYN_TASK) {
286 		if (MBDynNode[count] == ptrstr->node
287 			&& MBDynName[count] == nam2num(mbd_name))
288 		{
289 			mbdtask = (void *)RT_get_adr(ptrstr->node,
290 				ptrstr->port, mbd_name);
291 			ptrstr->mbdtask_count = count;
292 			break;
293 		}
294 
295 		if (MBDynNode[count] == 1 && MBDynName[count] == 0xFFFFFFFF) {
296 			i = WAIT_LOOP;
297 			printf("\nwrite: Connecting to MBDyn task\n");
298 
299 			while (!(mbdtask = (void *)RT_get_adr(ptrstr->node,
300 				ptrstr->port,mbd_name)))
301 			{
302 				if (!i) {
303 					snprintf(errMsg, sizeof(errMsg),
304 						"\ncannot find MBDyn task\n");
305 					ssSetErrorStatus(S, errMsg);
306 					printf("%s", errMsg);
307 					return;
308 				}
309 				rt_sleep(nano2count(1000000));
310 				i--;
311 			}
312 
313 			MBDynNode[count] = ptrstr->node;
314 			MBDynName[count] = nam2num(mbd_name);
315 			MBDynTaskActive[count] = true;
316 			ptrstr->mbdtask_count = count;
317 			break;
318 		}
319 
320 		count++;
321 	}
322 
323 	if (count == MAX_MBDYN_TASK) {
324 		snprintf(errMsg, sizeof(errMsg), "\nToo many MBDyn tasks\n");
325 		ssSetErrorStatus(S, errMsg);
326 		printf("%s", errMsg);
327 		return;
328 	}
329 
330 	/******************************************************************
331 	 * find mailbox
332 	 ******************************************************************/
333 
334 	i = WAIT_LOOP;
335 
336 	while (!(ptrstr->comptr = (void *)RT_get_adr(ptrstr->node,
337 		ptrstr->port, mbx_name)))
338 	{
339 		if (!i) {
340 			snprintf(errMsg, sizeof(errMsg),
341 				"\ncannot find output mailbox %s\n",
342 				mbx_name);
343 			ssSetErrorStatus(S, errMsg);
344 			printf("%s", errMsg);
345 			return;
346 		}
347 		rt_sleep(nano2count(1000000));
348 		i--;
349 	}
350 	printf("\nsfun_mbdyn_com_write:\n\n");
351 	printf("Host name: %s\n", host_name);
352 	printf("node: %lx\n", ptrstr->node);
353 	printf("port: %d\n", ptrstr->port);
354 	printf("mbdtask: %p\n", mbdtask);
355 	printf("mbx name: %s\n", mbx_name);
356 	printf("mbx: %p\n", ptrstr->comptr);
357 	printf("mbx channel: %d\n", MBX_N_CHN);
358 	printf("NANO_SAMPLE_TIME: %ld\n", NANO_SAMPLE_TIME);
359 
360 	if (timerflag) {
361 		stop_rt_timer();
362 	}
363 
364 #else
365 	/* inizializza la socket */
366 	int sock = 0;
367 	int conn = 0;
368 	/* salva la socket */
369 	ssGetIWork(S)[0] = (int_T)sock;
370 	/* imposta conn a 0 */
371 	ssGetIWork(S)[1] = (int_T)conn;
372 #endif /* MATLAB_MEX_FILE */
373 }
374 #endif /* MDL_START */
375 
376 static void
mdlOutputs(SimStruct * S,int_T tid)377 mdlOutputs(SimStruct *S, int_T tid)
378 {
379 #ifndef MATLAB_MEX_FILE
380 	register int	i;
381 	double		y[MBX_N_CHN];
382 	struct mbd_t	*ptrstr = (struct mbd_t *)ssGetPWork(S)[0];
383 
384 	for (i = 0; i < MBX_N_CHN; i++) {
385 		y[i] = ssGetInputPortRealSignal(S, i)[0];
386 		ssGetOutputPortRealSignal(S, i)[0] = y[i];
387 	}
388 
389 	if (RT_mbx_send_if(ptrstr->node, ptrstr->port, ptrstr->comptr, y,
390 		sizeof(double)*MBX_N_CHN) < 0)
391 	{
392 		MBDynTaskActive[ptrstr->mbdtask_count] = false;
393 		rt_send(rt_HostInterfaceTask, (int)msg);
394 	}
395 #else
396 	register int	i;
397 	double		y[MBX_N_CHN];
398 	int		sock = ssGetIWork(S)[0];
399 	int		conn = ssGetIWork(S)[1];
400 	static char_T	errMsg[BUFSIZ];
401 	int		save_errno;
402 
403 	if (conn == 0) {
404 		if (sock == 0) {
405 			if (NET) {
406 				/* usa il protocollo tcp/ip */
407 				struct sockaddr_in	addr;
408 				char			*host = NULL;
409 				int			flags;
410 
411 				/* crea una socket */
412 				sock = socket(PF_INET, SOCK_STREAM, 0);
413 				if (sock < 0) {
414 					snprintf(errMsg, sizeof(errMsg),
415 						"\nWRITE sfunction: unable to create a INET socket\n");
416 					ssSetErrorStatus(S, errMsg);
417 					printf("%s", errMsg);
418 					return;
419 				}
420 
421 				/* imposta la socket come non bloccante */
422         			flags = fcntl(sock, F_GETFL, 0);
423 				if (flags == -1) {
424 					snprintf(errMsg, sizeof(errMsg),
425 						"\nWRITE sfunction: unable to get socket flags\n");
426 					ssSetErrorStatus(S, errMsg);
427 					printf("%s", errMsg);
428 					return;
429 				}
430 
431 				flags |= O_NONBLOCK;
432 				if (fcntl(sock, F_SETFL, flags) == -1) {
433 					snprintf(errMsg, sizeof(errMsg),
434 						"\nWRITE sfunction: unable to set socket flags\n");
435 					ssSetErrorStatus(S, errMsg);
436 					printf("%s", errMsg);
437 					return;
438 				}
439 
440 				/* get host data */
441 				host = mxArrayToString(HOST_NAME_PARAM);
442 				if (host == NULL ) {
443 					snprintf(errMsg, sizeof(errMsg),
444 						"\nWRITE sfunction: unable to get host parameter\n");
445 					ssSetErrorStatus(S, errMsg);
446 					printf("%s", errMsg);
447 					return;
448 				}
449 
450 				addr.sin_family = AF_INET;
451 				addr.sin_port = htons(PORT);
452 				if (inet_aton(host, &addr.sin_addr) == 0) {
453 					snprintf(errMsg, sizeof(errMsg),
454 						"\nWRITE sfunction: unknown host '%s'\n", host);
455 					ssSetErrorStatus(S, errMsg);
456 					printf("%s", errMsg);
457 					mxFree(host);
458 					return;
459 				}
460 				mxFree(host);
461 
462 				/* connect */
463 				if (connect(sock, (struct sockaddr *)&addr, sizeof(addr)) != -1) {
464 					conn = 1;
465 					/* reimposta la socket come bloccante */
466 					flags &= (~O_NONBLOCK);
467 					if (fcntl(sock, F_SETFL, flags) == -1) {
468 						snprintf(errMsg, sizeof(errMsg),
469 							"\nWRITE sfunction: unable to set socket flags\n");
470 						ssSetErrorStatus(S, errMsg);
471 						printf("%s", errMsg);
472 						return;
473 					}
474 				}
475 
476 			} else {
477 				/* usa le socket local */
478 				struct sockaddr_un	*addrp;
479 				char			*path = NULL;
480 				int			flags;
481 				size_t			len, size;
482 
483 				/* crea una socket */
484 				sock = socket(PF_LOCAL, SOCK_STREAM, 0);
485 				if (sock < 0) {
486 					snprintf(errMsg, sizeof(errMsg),
487 						"\nWRITE sfunction: unable to create a LOCAL socket\n");
488 					ssSetErrorStatus(S, errMsg);
489 					printf("%s", errMsg);
490 					return;
491 				}
492 
493 				/* imposta la socket come non bloccante */
494 	        		flags = fcntl(sock, F_GETFL, 0);
495 	        		if (flags == -1) {
496 					snprintf(errMsg, sizeof(errMsg),
497 						"\nWRITE sfunction: unable to get socket flags\n");
498 					ssSetErrorStatus(S, errMsg);
499 					printf("%s", errMsg);
500 					return;
501 				}
502 
503 				flags |= O_NONBLOCK;
504 				if (fcntl(sock, F_SETFL, flags) == -1) {
505 					snprintf(errMsg, sizeof(errMsg),
506 						"\nWRITE sfunction: unable to set socket flags\n");
507 					ssSetErrorStatus(S, errMsg);
508 					printf("%s", errMsg);
509 					return;
510 				}
511 
512 				/* get path data */
513 				path = mxArrayToString(PATH_PARAM);
514 				if (path == NULL ) {
515 					snprintf(errMsg, sizeof(errMsg),
516 						"\nWRITE sfunction: unable to get path parameter\n");
517 					ssSetErrorStatus(S, errMsg);
518 					printf("%s", errMsg);
519 					return;
520 				}
521 
522 				len = strlen(path);
523 				addrp = malloc(sizeof(struct sockaddr_un) + len + 1);
524 				addrp->sun_family = AF_LOCAL;
525 				strncpy(addrp->sun_path, path, len);
526 				mxFree(path);
527 
528 				addrp->sun_path[len] = '\0';
529 				size = (offsetof (struct sockaddr_un, sun_path) + len + 1);
530 
531 				/* connect */
532 				if (connect(sock, (struct sockaddr *)addrp, size) == 0) {
533 					conn = 1;
534 					/* reimposta la socket come bloccante */
535 					flags &= (~O_NONBLOCK);
536 					if (fcntl(sock, F_SETFL, flags) == -1) {
537 						snprintf(errMsg, sizeof(errMsg),
538 							"\nWRITE sfunction: unable to set socket flags\n");
539 						ssSetErrorStatus(S, errMsg);
540 						printf("%s", errMsg);
541 						return;
542 					}
543 				}
544 			}
545 
546 			/* salva la socket */
547 			ssGetIWork(S)[0] = (int_T)sock;
548 
549 		} else {
550 			/* poll */
551 			struct pollfd	ufds;
552 			int		rc;
553 			int		retries = 0;
554 
555 retry:;
556 			ufds.fd = sock;
557 			ufds.events = (POLLIN | POLLOUT);
558 			rc = poll(&ufds, 1, TIME_OUT);
559 			switch (rc) {
560 			case -1:
561 				save_errno = errno;
562 				snprintf(errMsg, sizeof(errMsg),
563 					"\nWRITE sfunction: POLL error (%d: %s)\n",
564 					save_errno, strerror(save_errno));
565 				ssSetErrorStatus(S, errMsg);
566 				printf("%s", errMsg);
567 				return;
568 
569 			case 0:
570 				snprintf(errMsg, sizeof(errMsg),
571 					"\nWRITE sfunction: connection timeout reached\n");
572 				ssSetErrorStatus(S, errMsg);
573 				printf("%s", errMsg);
574 				return;
575 
576 			default :
577 			{
578 				int	flags;
579 
580 				if ((ufds.revents & POLLHUP) && ++retries < 200) {
581 					usleep(10000);
582 					goto retry;
583 				}
584 
585 				if (ufds.revents & (POLLERR | POLLHUP | POLLNVAL)) {
586 					snprintf(errMsg, sizeof(errMsg),
587 						"\nWRITE sfunction: POLL error "
588 						"(%d ERR=%d HUP=%d NVAL=%d)\n",
589 						rc,
590 						ufds.revents & POLLERR,
591 						ufds.revents & POLLHUP,
592 						ufds.revents & POLLNVAL);
593 					ssSetErrorStatus(S, errMsg);
594 					printf("%s", errMsg);
595 					return;
596 				}
597 
598 				conn = 1;
599 				/* reimposta la socket come bloccante */
600         			flags = fcntl(sock, F_GETFL, 0);
601         			if (flags == -1) {
602 					snprintf(errMsg, sizeof(errMsg),
603 						"\nWRITE sfunction: unable to get socket flags\n");
604 					ssSetErrorStatus(S, errMsg);
605 					printf("%s", errMsg);
606 					return;
607 				}
608 				flags &= (~O_NONBLOCK);
609 				if (fcntl(sock, F_SETFL, flags) == -1) {
610 					snprintf(errMsg, sizeof(errMsg),
611 						"\nWRITE sfunction: unable to set socket flags\n");
612 					ssSetErrorStatus(S, errMsg);
613 					printf("%s", errMsg);
614 					return;
615 				}
616 			}
617 			}
618 			/* imposta conn a 1 */
619 
620 		} /* socket == 0 */
621 		ssGetIWork(S)[1] = (int_T)conn;
622 	} /* conn == 0 */
623 
624 	if (conn) {
625 		/* scrive sulla socket */
626 		for (i = 0; i < MBX_N_CHN; i++) {
627 			y[i] = ssGetInputPortRealSignal(S, i)[0];
628 			ssGetOutputPortRealSignal(S, i)[0] = y[i];
629 		}
630 		if (send(sock, y, sizeof(double)*MBX_N_CHN, 0) == -1) {
631 			snprintf(errMsg, sizeof(errMsg),
632 				"\nWriteSfunction: Communication closed by host\n");
633 			ssSetStopRequested(S, 1);
634 			printf("%s", errMsg);
635 			return;
636 		}
637 	}
638 #endif /* MATLAB_MEX_FILE */
639 }
640 
641 static void
mdlTerminate(SimStruct * S)642 mdlTerminate(SimStruct *S)
643 {
644 #ifndef MATLAB_MEX_FILE
645 	struct 	mbd_t *ptrstr = (struct mbd_t *)ssGetPWork(S)[0];
646 	void	*mbdtask;
647 	char 	mbd_name[7];
648 
649 	printf("\nsfun_mbdyn_com_write, %s:\n", ssGetModelName(S));
650 
651 	if (ptrstr) {
652 		/****************************************************************
653 		 * terminate mbdyn task
654 		 ****************************************************************/
655 		if (MBDynTaskActive[ptrstr->mbdtask_count]) {
656 			mxGetString(MBD_NAME_PARAM, mbd_name, 7);
657 			mbdtask = (void *)RT_get_adr(ptrstr->node,
658 				ptrstr->port, mbd_name);
659 			printf("MBDyn is stopped by write s-function\n");
660 			RT_send_timed(ptrstr->node, ptrstr->port, mbdtask, 1, NANO_SAMPLE_TIME*2);
661 			MBDynTaskActive[ptrstr->mbdtask_count] = false;
662 		}
663 		/****************************************************************
664 		 * release port
665 		 ****************************************************************/
666 
667 		if (ptrstr->node) {
668 			rt_release_port(ptrstr->node, ptrstr->port);
669 		}
670 
671 		/****************************************************************
672 		 * free structure memory
673 		 ****************************************************************/
674 		free(ptrstr);
675 	}
676 #else
677 	int sock = ssGetIWork(S)[0];
678 #ifdef VERBOSE
679 	fprintf(stderr, "WRITE: closing socket...\n");
680 #endif /* VERBOSE */
681 	/* chiude la socket */
682 	shutdown(sock, SHUT_RDWR);
683 #endif /* MATLAB_MEX_FILE */
684 }
685 
686 #ifdef  MATLAB_MEX_FILE
687 #include "simulink.c"
688 #else
689 #include "cg_sfun.h"
690 #endif
691 
692