API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 13th, 2013
51
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.21 KB | None | 0 0
raw download clone embed print report
  1.  
  2. //
  3. // Driver for the Mesa SSI Encoder module.
  4. // It is expected that it will be expanded to cover BISS and Fanuc absolute
  5. // encoders in the future.
  6. //
  7.  
  8.  
  9. #include <linux/slab.h>
  10.  
  11. #include "rtapi.h"
  12. #include "rtapi_app.h"
  13. #include "rtapi_string.h"
  14. #include "rtapi_math.h"
  15. #include "rtapi_math64.h"
  16. #include "hal.h"
  17. #include "hal/drivers/mesa-hostmot2/hostmot2.h"
  18.  
  19.  
  20. static void hm2_absenc_trigger(void *void_hm2, long period){
  21. hostmot2_t *hm2 = void_hm2;
  22. u32 buff = 0xFFFFFFFF;
  23. hm2->llio->write(hm2->llio,
  24. hm2->absenc.ssi_global_start_addr,
  25. &buff,
  26. sizeof(u32));
  27. }
  28.  
  29.  
  30. int hm2_absenc_parse_md(hostmot2_t *hm2, int md_index,
  31. char all_formats[][MAX_ABSENC_LEN]) {
  32. hm2_module_descriptor_t *md = &hm2->md[md_index];
  33. static int last_index = -1;
  34. static int last_format = 0;
  35. static int funct_flag = 0;
  36. int i, r = 0, f = 0;
  37. int index;
  38.  
  39. rtapi_print("\nlast_index = %i\n", last_index);
  40.  
  41. //
  42. // some standard sanity checks
  43. //
  44. if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 4, 4, 0x03)) {
  45. HM2_ERR("inconsistent absenc Module Descriptor!\n");
  46. return -EINVAL;
  47. }
  48. /* This module is intended to handle more than just SSI, so re-entering is OK
  49. */
  50. for(i = 0; i < MAX_ABSENCS; i++){
  51. if (all_formats[i][0]){
  52. if ( i >= md->instances) {
  53. HM2_ERR( "config.num_absenc=%d, but only %d are available, not"
  54. " loading driver\n",
  55. hm2->config.num_absencs, md->instances );
  56. return -EINVAL;
  57. }
  58. hm2->absenc.num_chans = i + 1;
  59. }
  60. }
  61. if (hm2->absenc.num_chans == 0) {
  62. return 0;
  63. }
  64.  
  65. //
  66. // looks good, start initializing
  67. //
  68.  
  69. hm2->absenc.clock_frequency = md->clock_freq;
  70. index = last_index;
  71.  
  72. for (f = 0; f < MAX_ABSENCS; f++){
  73. char name[HM2_SSERIAL_MAX_STRING_LENGTH+1] = "";
  74. char* format = all_formats[f];
  75. hm2_sserial_remote_t *chan;
  76. int bitcount = 0;
  77. if (*format){
  78. index++;
  79. hm2->absenc.chans = (hm2_sserial_remote_t *)krealloc(hm2->absenc.chans,
  80. (index + 1) * sizeof(hm2_sserial_remote_t),
  81. GFP_KERNEL);
  82. chan = &hm2->absenc.chans[index];
  83. chan->num_confs = 0;
  84. while(*format){
  85. if (*format == '%' && *name){
  86. int q = simple_strtol(++format, &format, 0);
  87. bitcount += q;
  88. if (q == 0){
  89. HM2_ERR("Invalid field length specification, you may "
  90. "not get the pins you expected\n");
  91. }
  92. else if (strchr("bBuUsSeEfFpPgG", *format)){
  93. hm2_sserial_data_t *conf;
  94. chan->num_confs++;
  95. chan->confs = (hm2_sserial_data_t *)
  96. krealloc(chan->confs,
  97. chan->num_confs * sizeof(hm2_sserial_data_t),
  98. GFP_KERNEL);
  99.  
  100. conf = &chan->confs[chan->num_confs - 1];
  101. if (conf == NULL){ HM2_ERR("Oh dash and blast it\n") ; return -1;}
  102. conf->DataDir = LBP_IN;
  103. conf->DataLength = q;
  104. strcpy(conf->NameString, name);
  105. conf->RecordType = 0xA0;
  106. conf->ParmAddr = 0;
  107. strcpy(conf->UnitString, "none");
  108.  
  109. switch(*format){
  110. case 'b':
  111. case 'B':
  112. conf->DataType = LBP_BITS;
  113. conf->ParmMax = 0;
  114. conf->ParmMin = 0;
  115. break;
  116. case 'u':
  117. case 'U':
  118. conf->DataType = LBP_UNSIGNED;
  119. conf->ParmMax = 0;
  120. conf->ParmMin = 0;
  121. break;
  122. case 's':
  123. case 'S':
  124. conf->DataType = LBP_SIGNED;
  125. conf->ParmMax = 0;
  126. conf->ParmMin = 0;
  127. break;
  128. case 'f':
  129. case 'F':
  130. conf->DataType = LBP_BOOLEAN;
  131. conf->ParmMax = 0;
  132. conf->ParmMin = 0;
  133. break;
  134. case 'p':
  135. case 'P':
  136. conf->DataType = LBP_PAD;
  137. conf->ParmMax = 0;
  138. conf->ParmMin = 0;
  139. break;
  140. case 'g':
  141. case 'G':
  142. strcpy(conf->UnitString, "gray");
  143. /* no break */
  144. case 'e':
  145. case 'E':
  146. conf->DataType = LBP_ENCODER;
  147. conf->ParmMax = 0;
  148. conf->ParmMin = 0;
  149. break;
  150. }
  151. }
  152. else
  153. {
  154. HM2_ERR("Unknown format specifer %s\n", format);
  155. }
  156. //Start a new name
  157. strcpy(name, "");
  158. //move to the next string
  159. format++;
  160. }
  161. else
  162. {
  163. strncat(name, format++, 1);
  164. }
  165. }
  166.  
  167. switch (md->gtag){
  168. case HM2_GTAG_SSI:
  169. rtapi_snprintf(chan->name, sizeof(chan->name),
  170. "%s.ssi.%02d", hm2->llio->name, f);
  171. if ( hm2_sserial_create_pins(hm2, chan)) goto fail1;
  172. chan->params = hal_malloc(sizeof(hm2_sserial_params_t));
  173. hm2->absenc.clock_frequency = md->clock_freq;
  174. hm2->absenc.ssi_version = md->version;
  175.  
  176. chan->reg_0_addr = md->base_address + (0 * md->register_stride);
  177. chan->reg_1_addr = md->base_address + (1 * md->register_stride);
  178. chan->reg_cs_addr = md->base_address + (2 * md->register_stride);
  179. chan->command_reg_addr = 0; // Just used as a handy 32-bit buffer
  180. if (hm2->absenc.ssi_global_start_addr == 0){
  181. hm2->absenc.ssi_global_start_addr = md->base_address + (3 * md->register_stride);
  182. }
  183.  
  184. r = hm2_register_tram_read_region(hm2, chan->reg_cs_addr,
  185. sizeof(u32),
  186. &chan->reg_cs_read); // To check "busy"
  187. r += hm2_register_tram_read_region(hm2, chan->reg_0_addr,
  188. sizeof(u32),
  189. &chan->reg_0_read);
  190. r += hm2_register_tram_read_region(hm2, chan->reg_1_addr,
  191. sizeof(u32),
  192. &chan->reg_1_read);
  193. if (r < 0) {
  194. HM2_ERR("error registering tram read region for SSI encoder\n");
  195. goto fail1;
  196. }
  197.  
  198. rtapi_snprintf(name, sizeof(name), "%s.ssi.%02d.frequency-khz",
  199. hm2->llio->name, f);
  200. r = hal_param_float_new(name, HAL_RW,
  201. &(chan->params->float_param ),
  202. hm2->llio->comp_id);
  203. if (r < 0) {
  204. HM2_ERR("error adding param '%s', aborting\n", name);
  205. goto fail1;
  206. }
  207.  
  208. // export the absenc to HAL
  209. //Unlike most other Hostmot2 functions, this one needs to have a private
  210. // "trigger" function directly callable from HAL.
  211. if (! (funct_flag & 0x01)){
  212. rtapi_snprintf(name, sizeof(name),
  213. "%s.trigger-ssi-encoders", hm2->llio->name);
  214. hal_export_funct(name, hm2_absenc_trigger,
  215. hm2, 0, 0,hm2->llio->comp_id);
  216. funct_flag |= 0x01;
  217. }
  218.  
  219. break;
  220. default:
  221. HM2_ERR("We don't handle BISS or Fanuc yet, sorry\n");
  222. }
  223. }
  224. }
  225. //Store the indexes for re-entry to handle other encoder styles
  226. //On balance I think we need one set of formats per encoder style
  227. //just because of how the mds are sent through.
  228.  
  229. last_index = index;
  230. last_format = f;
  231.  
  232. return hm2->absenc.num_chans;
  233.  
  234. fail1:
  235. hm2_absenc_cleanup(hm2);
  236.  
  237. hm2->absenc.num_chans = 0;
  238. return r;
  239. }
  240.  
  241.  
  242. void hm2_absenc_process_tram_read(hostmot2_t *hm2, long period) {
  243. int i;
  244.  
  245. static int err_count = 0;
  246. if (hm2->absenc.num_chans <= 0) return;
  247.  
  248. // process each absenc instance independently
  249. for (i = 0; i < hm2->absenc.num_chans; i ++) {
  250. hm2_sserial_remote_t *chan = &hm2->absenc.chans[i];
  251. // Check that the data is all in.
  252. if ((*chan->reg_cs_read & 0x80000000) && err_count < 10){
  253. err_count++;
  254. HM2_ERR("Data transmission not complete before encoder position \n"
  255. "read You may need to place the absenc_trigger function \n"
  256. "earlier in the thread (warning %i of 10)\n", err_count);
  257. }
  258. else
  259. {
  260. hm2_sserial_read_pins(chan);
  261. }
  262. }
  263. }
  264.  
  265. void hm2_absenc_write(hostmot2_t *hm2){
  266.  
  267. int i;
  268. u32 buff;
  269. if (hm2->absenc.num_chans <= 0) return;
  270.  
  271. for (i = 0; i < hm2->absenc.num_chans; i ++) {
  272. for (i = 0; i < hm2->absenc.num_chans; i ++) {
  273. hm2_sserial_remote_t *chan = &hm2->absenc.chans[i];
  274. buff = ((u32)(0x10000 * (chan->params->float_param * 1000
  275. / hm2->absenc.clock_frequency))) << 16
  276. | 0x100 //enable global read
  277. | chan->num_read_bits;
  278. //This is NOT the command reg address, it is a re-purposed u32
  279. if (buff != chan->command_reg_addr ){
  280. rtapi_print("frequency %d bits %i buff%x\n", hm2->absenc.clock_frequency,chan->num_read_bits, buff );
  281. rtapi_print("address%x\n", chan->reg_cs_addr );
  282. hm2->llio->write(hm2->llio,
  283. chan->reg_cs_addr,
  284. &buff,
  285. sizeof(u32));
  286. chan->command_reg_addr = buff;
  287. }
  288. }
  289. }
  290. }
  291.  
  292. void hm2_absenc_cleanup(hostmot2_t *hm2) {
  293. int i;
  294. rtapi_print("FIXME: !! Exiting without cleanup! \n");
  295. return;
  296. if (hm2->absenc.chans != NULL) {
  297. for (i = 0 ; i < hm2->absenc.num_chans ; i++){
  298. if (hm2->absenc.chans[i].confs != NULL){
  299. kfree(hm2->absenc.chans[i].confs);
  300. }
  301. }
  302. kfree(hm2->absenc.chans);
  303. }
  304. }
  305.  
  306.  
  307. void hm2_absenc_print_module(hostmot2_t *hm2) {
  308. int i;
  309. HM2_PRINT("absenc: %d\n", hm2->absenc.num_chans);
  310. if (hm2->absenc.num_chans <= 0) return;
  311. HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n",
  312. hm2->absenc.clock_frequency,
  313. hm2_hz_to_mhz(hm2->absenc.clock_frequency));
  314. HM2_PRINT(" ssi-version: %d\n", hm2->absenc.ssi_version);
  315. HM2_PRINT(" biss-version: %d\n", hm2->absenc.biss_version);
  316. HM2_PRINT(" fanuc-version: %d\n", hm2->absenc.fanuc_version);
  317. for (i = 0; i < hm2->absenc.num_chans; i ++) {
  318. HM2_PRINT(" instance %d:\n", i);
  319. HM2_PRINT(" hw:\n");
  320. HM2_PRINT(" command_addr: 0x%04X\n", hm2->absenc.chans[i].reg_cs_addr);
  321. HM2_PRINT(" data 0 addr: 0x%04X\n", hm2->absenc.chans[i].reg_0_addr);
  322. HM2_PRINT(" data 1 addr: 0x%04X\n", hm2->absenc.chans[i].reg_1_addr);
  323. HM2_PRINT(" data 2 addr: 0x%04X\n", hm2->absenc.chans[i].reg_2_addr);
  324. }
  325. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!