mux_generic.c
1 // Copyright (C) 2013 Andy Pugh 2 3 // This program is free software; you can redistribute it and/or modify 4 // it under the terms of the GNU General Public License as published by 5 // the Free Software Foundation; either version 2 of the License, or 6 // (at your option) any later version. 7 // 8 // This program is distributed in the hope that it will be useful, 9 // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 // GNU General Public License for more details. 12 // 13 // You should have received a copy of the GNU General Public License 14 // along with this program; if not, write to the Free Software 15 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 16 // 17 18 // A generic/configurable multiplexer component 19 20 #include "rtapi.h" 21 #include "rtapi_app.h" 22 #include "hal.h" 23 #include "hal_priv.h" 24 25 #if !defined(__KERNEL__) 26 #include <stdio.h> 27 #include <stdlib.h> 28 #endif 29 30 /* module information */ 31 MODULE_AUTHOR("Andy Pugh"); 32 MODULE_DESCRIPTION("Generic mux component for linuxCNC"); 33 MODULE_LICENSE("GPL"); 34 35 #define MAX_CHAN 100 36 #define MAX_SIZE 1024 37 #define EPS 2e-7 38 #define MAX_S32 0x7FFFFFFF 39 #define MAX_U32 0xFFFFFFFF 40 41 typedef struct { 42 hal_data_u **inputs; 43 hal_data_u *output; 44 hal_u32_t *sel_int; 45 hal_bit_t **sel_bit; 46 unsigned int selection; 47 hal_u32_t *debounce; 48 unsigned int timer; 49 hal_bit_t *suppress; 50 int in_type; 51 int out_type; 52 int size; 53 int num_bits; 54 } mux_inst_t; 55 56 typedef struct { 57 mux_inst_t *insts; 58 int num_insts; 59 } mux_t; 60 61 static int comp_id; 62 static mux_t *mux; 63 static void write_fp(void *arg, long period); 64 static void write_nofp(void *arg, long period); 65 66 char *config[MAX_CHAN]; 67 RTAPI_MP_ARRAY_STRING(config, MAX_CHAN, "mux specifiers inNUMout"); 68 69 int rtapi_app_main(void){ 70 int retval; 71 int i, f; 72 char hal_name[HAL_NAME_LEN]; 73 char *types[5] = {"invalid", "bit", "float", "s32", "u32"}; 74 if (!config[0]) { 75 rtapi_print_msg(RTAPI_MSG_ERR, "The mux_generic component requires at least" 76 " one valid format string\n"); 77 return -EINVAL; 78 } 79 80 comp_id = hal_init("mux_generic"); 81 if (comp_id < 0) { 82 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: hal_init() failed\n"); 83 return -1; 84 } 85 86 // allocate shared memory for the base struct 87 mux = hal_malloc(sizeof(mux_t)); 88 if (mux == 0) { 89 rtapi_print_msg(RTAPI_MSG_ERR, 90 "mux_generic component: Out of Memory\n"); 91 hal_exit(comp_id); 92 return -1; 93 } 94 95 // Count the instances. 96 for (mux->num_insts = 0; config[mux->num_insts];mux->num_insts++) {} 97 mux->insts = hal_malloc(mux->num_insts * sizeof(mux_inst_t)); 98 // Parse the config string 99 for (i = 0; i < mux->num_insts; i++) { 100 char c; 101 int s, p = 0; 102 mux_inst_t *inst = &mux->insts[i]; 103 inst->in_type = -1; 104 inst->out_type = -1; 105 for (f = 0; (c = config[i][f]); f++) { 106 int type; 107 type = 0; 108 switch (c) { 109 case '0': 110 case '1': 111 case '2': 112 case '3': 113 case '4': 114 case '5': 115 case '6': 116 case '7': 117 case '8': 118 case '9': 119 inst->size = (inst->size * 10) + (c - '0'); 120 if (inst->size > MAX_SIZE) inst->size = MAX_SIZE; 121 break; 122 case 'b': 123 case 'B': 124 type = HAL_BIT; 125 break; 126 case 'f': 127 case 'F': 128 type = HAL_FLOAT; 129 break; 130 case 's': 131 case 'S': 132 type = HAL_S32; 133 break; 134 case 'u': 135 case 'U': 136 type = HAL_U32; 137 break; 138 default: 139 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: invalid character in " 140 "fmt string\n"); 141 goto fail0; 142 } 143 if (type) { 144 if (inst->in_type == -1) { 145 inst->in_type = type; 146 } 147 else if (inst->out_type == -1) { 148 inst->out_type = type; 149 } 150 else 151 { 152 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: too many type " 153 "specifiers in fmt string\n"); 154 goto fail0; 155 } 156 } 157 } 158 if (inst->size < 1) { 159 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No entry count given\n"); 160 goto fail0; 161 } 162 else if (inst->size < 2) { 163 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: A one-element mux makes " 164 "no sense\n"); 165 goto fail0; 166 } 167 if (inst->in_type == -1) { 168 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: No type specifiers in " 169 "fmt string\n"); 170 goto fail0; 171 } 172 else if (inst->out_type == -1) { 173 inst->out_type = inst->in_type; 174 } 175 176 retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, "mux-gen.%02i", i); 177 if (retval >= HAL_NAME_LEN) { 178 goto fail0; 179 } 180 if (inst->in_type == HAL_FLOAT || inst->out_type == HAL_FLOAT) { 181 retval = hal_export_funct(hal_name, write_fp, inst, 1, 0, comp_id); 182 if (retval < 0) { 183 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export" 184 " failed\n"); 185 goto fail0; 186 } 187 } 188 else 189 { 190 retval = hal_export_funct(hal_name, write_nofp, inst, 0, 0, comp_id); 191 if (retval < 0) { 192 rtapi_print_msg(RTAPI_MSG_ERR, "mux_generic: ERROR: function export" 193 " failed\n"); 194 goto fail0; 195 } 196 } 197 198 // Input pins 199 200 // if the mux size is a power of 2 then create the bit inputs 201 s = inst->size; 202 for(inst->num_bits = 1; (!((s >>= 1) & 1)); inst->num_bits++); 203 if (s !=1){ 204 inst->num_bits = 0; 205 } else { //make the bit pins 206 inst->sel_bit = hal_malloc(inst->num_bits * sizeof(hal_bit_t*)); 207 for (p = 0; p < inst->num_bits; p++) { 208 retval = hal_pin_bit_newf(HAL_IN, &inst->sel_bit[p], comp_id, 209 "mux-gen.%02i.sel-bit-%02i", i, p); 210 if (retval != 0) { 211 goto fail0; 212 } 213 } 214 } 215 216 retval = hal_pin_u32_newf(HAL_IN, &(inst->sel_int), comp_id, 217 "mux-gen.%02i.sel-int", i); 218 if (retval != 0) { 219 goto fail0; 220 } 221 222 inst->inputs = hal_malloc(inst->size * sizeof(hal_data_u*)); 223 for (p = 0; p < inst->size; p++) { 224 retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, 225 "mux-gen.%02i.in-%s-%02i", i, types[inst->in_type], p); 226 if (retval >= HAL_NAME_LEN) { 227 goto fail0; 228 } 229 retval = hal_pin_new(hal_name, inst->in_type, HAL_IN, 230 (void**)&(inst->inputs[p]), comp_id); 231 if (retval != 0) { 232 goto fail0; 233 } 234 } 235 236 // Behaviour-modifiers 237 retval = hal_pin_bit_newf(HAL_IN, &inst->suppress, comp_id, 238 "mux-gen.%02i.suppress-no-input", i); 239 if (retval != 0) { 240 goto fail0; 241 } 242 retval = hal_pin_u32_newf(HAL_IN, &inst->debounce, comp_id, 243 "mux-gen.%02i.debounce-us", i); 244 if (retval != 0) { 245 goto fail0; 246 } 247 retval = hal_param_u32_newf(HAL_RO, &inst->timer, comp_id, 248 "mux-gen.%02i.elapsed", i); 249 if (retval != 0) { 250 goto fail0; 251 } 252 retval = hal_param_u32_newf(HAL_RO, &inst->selection, comp_id, 253 "mux-gen.%02i.selected", i); 254 if (retval != 0) { 255 goto fail0; 256 } 257 258 //output pins 259 retval = rtapi_snprintf(hal_name, HAL_NAME_LEN, 260 "mux-gen.%02i.out-%s", i, types[inst->out_type]); 261 if (retval >= HAL_NAME_LEN) { 262 goto fail0; 263 } 264 retval = hal_pin_new(hal_name, inst->out_type, HAL_OUT, 265 (void**)&(inst->output), comp_id); 266 if (retval != 0) { 267 goto fail0; 268 } 269 270 } 271 272 hal_ready(comp_id); 273 return 0; 274 275 fail0: 276 hal_exit(comp_id); 277 return -1; 278 279 } 280 281 void write_fp(void *arg, long period) { 282 mux_inst_t *inst = arg; 283 int i = 0, s = 0; 284 if (inst->num_bits > 0) { 285 while (i < inst->num_bits) { 286 s += (*inst->sel_bit[i] != 0) << i; 287 i++; 288 } 289 } 290 // if you document it, it's not a bug, it's a feature. Might even be useful 291 s += *inst->sel_int; 292 293 if (*inst->suppress && s == 0) 294 return; 295 if (s != inst->selection && inst->timer < *inst->debounce) { 296 inst->timer += period / 1000; 297 return; 298 } 299 300 inst->selection = s; 301 inst->timer = 0; 302 303 if (s >= inst->size) 304 s = inst->size - 1; 305 306 switch (inst->in_type * 8 + inst->out_type) { 307 case 012: //HAL_BIT => HAL_FLOAT 308 inst->output->f = inst->inputs[s]->b ? 1.0 : 0.0; // 309 break; 310 case 021: //HAL_FLOAT => HAL_BIT 311 inst->output->b = 312 (inst->inputs[s]->f > EPS || inst->inputs[s]->f < -EPS) ? 1 : 0; 313 break; 314 case 022: //HAL_FLOAT => HAL_FLOAT 315 inst->output->f = inst->inputs[s]->f; 316 break; 317 case 023: //HAL_FLOAT => HAL_S32 318 if (inst->inputs[s]->f > MAX_S32) { 319 inst->output->s = MAX_S32; 320 } else if (inst->inputs[s]->f < -MAX_S32) { 321 inst->output->s = -MAX_S32; 322 } else { 323 inst->output->s = inst->inputs[s]->f; 324 } 325 break; 326 case 024: //HAL_FLOAT => HAL_U32 327 if (inst->inputs[s]->f > MAX_U32) { 328 inst->output->u = MAX_U32; 329 } else if (inst->inputs[s]->f < 0) { 330 inst->output->u = 0; 331 } else { 332 inst->output->u = inst->inputs[s]->f; 333 } 334 break; 335 case 032: //HAL_S32 => HAL_FLOAT 336 inst->output->f = inst->inputs[s]->s; 337 break; 338 case 042: //HAL_U32 => HAL_FLOAT 339 inst->output->f = (unsigned int) inst->inputs[s]->u; 340 break; 341 } 342 } 343 344 void write_nofp(void *arg, long period) { 345 mux_inst_t *inst = arg; 346 int i = 0, s = 0; 347 if (inst->num_bits > 0) { 348 while (i < inst->num_bits) { 349 s += (*inst->sel_bit[i] != 0) << i; 350 i++; 351 } 352 } 353 354 s += *inst->sel_int; 355 356 if (*inst->suppress && s == 0) 357 return; 358 if (s != inst->selection && inst->timer < *inst->debounce) { 359 inst->timer += period / 1000; 360 return; 361 } 362 363 inst->selection = s; 364 inst->timer = 0; 365 366 if (s >= inst->size) 367 s = inst->size - 1; 368 switch (inst->in_type * 8 + inst->out_type) { 369 case 011: //HAL_BIT => HAL_BIT 370 inst->output->b = inst->inputs[s]->b; 371 break; 372 case 013: //HAL_BIT => HAL_S32 373 inst->output->s = inst->inputs[s]->b; 374 break; 375 case 014: //HAL_BIT => HAL_U32 376 inst->output->u = inst->inputs[s]->b; 377 break; 378 case 031: //HAL_S32 => HAL_BIT 379 inst->output->b = inst->inputs[s]->s == 0 ? 0 : 1; 380 break; 381 case 033: //HAL_S32 => HAL_S32 382 inst->output->s = inst->inputs[s]->s; 383 break; 384 case 034: //HAL_S32 => HAL_U32 385 inst->output->u = (inst->inputs[s]->s > 0) ? inst->inputs[s]->s : 0; 386 break; 387 case 041: //HAL_U32 => HAL_BIT 388 inst->output->b = inst->inputs[s]->u == 0 ? 0 : 1; 389 break; 390 case 043: //HAL_U32 => HAL_S32 391 inst->output->s = 392 ((unsigned int) inst->inputs[s]->u > MAX_S32) ? 393 MAX_S32 : inst->inputs[s]->u; 394 break; 395 case 044: //HAL_U32 => HAL_U32 396 inst->output->u = inst->inputs[s]->u; 397 break; 398 } 399 } 400 401 void rtapi_app_exit(void){ 402 //everything is hal_malloc-ed which saves a lot of cleanup here 403 hal_exit(comp_id); 404 }