jr3_driver.cpp
Go to the documentation of this file.
00001 /*
00002  * ResMgr and Message Server Process
00003  *
00004  * This program is for JR3/Nitta Force moment sensor.
00005  * Copyright(C) by Waseda University, Nitta Coropration. 2002.
00006  *
00007  * Modified by Kei Okada, 2014
00008  */
00009 
00010 #include <stdio.h>
00011 #include <unistd.h>
00012 #include <stdlib.h>
00013 #include <errno.h>
00014 #include <string.h>
00015 #include <sys/time.h>
00016 #include <sys/neutrino.h>
00017 #include <sys/iofunc.h>
00018 #include <sys/dispatch.h>
00019 #include <sys/mman.h>
00020 #include <hw/pci.h>
00021 
00022 #define min(a, b) ((a) < (b) ? (a) : (b))
00023 
00024 int io_read (resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb);      //
00025 static char *buffer = (char *) "Hello world\n"; //
00026 
00027 void wait_t (void);             // wait for 3 sec.
00028 
00029 #include "QNX101/jr3pci3.idm"
00030 
00031 #define VENDORID        0x1762  /* Vendor ID of JR3 */
00032 // #define DEVICEID     0x3114  /* for 4ch board */
00033 // #define DEVICEID     0x3113  /* for 3ch board */
00034 #define DEVICEID        0x3112  /* for 2ch board */
00035 // #define DEVICEID     0x1111  /* for 1ch board */
00036 
00037 #define Jr3ResetAddr    0x18000
00038 #define Jr3NoAddrMask   0x40000
00039 #define Jr3DmAddrMask   0x6000
00040 unsigned long MappedAddress;
00041 
00042 /* Convert a DSP bus address to a PCI bus address */
00043 #define ToJr3PciAddrH(addr)     (((int)(addr) << 2) + Jr3BaseAddressH)
00044 #define ToJr3PciAddrL(addr)     (((int)(addr) << 2) + Jr3BaseAddressL)
00045 
00046 /* Read and Write from the program memory */
00047 #define ReadJr3Pm(addr)         (*(uint16_t volatile *)(ToJr3PciAddrH((addr))) << 8 |\
00048                                  *(uint8_t  volatile *)(ToJr3PciAddrL((addr))))
00049 
00050 #define WriteJr3Pm2(addr,data,data2)    (*(int volatile *)ToJr3PciAddrH((addr)) = (int)(data));(*(int volatile *)ToJr3PciAddrL((addr)) = (int)(data2))
00051 
00052 
00053 
00054 #define WriteJr3Pm(addr,data)           WriteJr3Pm2((addr),(data) >> 8, (data))
00055 
00056 /* Read and Write from the data memory using bus relative addressing */
00057 #define ReadJr3Dm(addr) (*(uint16_t volatile *)(ToJr3PciAddrH((addr))))
00058 #define WriteJr3Dm(addr,data)   *(int volatile *)(ToJr3PciAddrH((addr))) = (int)(data)
00059 
00060 /* Read and Write from the data memory using 0 relative addressing */
00061 #define ReadJr3(addr) (ReadJr3Dm((addr) | Jr3DmAddrMask))
00062 #define WriteJr3(addr,data) WriteJr3Dm((addr) | Jr3DmAddrMask,data)
00063 
00064 
00065 static resmgr_connect_funcs_t ConnectFuncs;     //
00066 static resmgr_io_funcs_t IoFuncs;       //
00067 static iofunc_attr_t IoFuncAttr;        //
00068 
00069 /* Jr3 Base address */
00070 volatile uint32_t Jr3BaseAddressH;
00071 volatile uint32_t Jr3BaseAddressL;
00072 volatile uint32_t Jr3BaseAddress0H;
00073 volatile uint32_t Jr3BaseAddress0L;
00074 volatile uint32_t Jr3BaseAddress1H;
00075 volatile uint32_t Jr3BaseAddress1L;
00076 
00077 /* Jr3 memory map */
00078 /*
00079  For the following structure definitions :
00080  int:          signed 16 bit value
00081  unsigned int: unsigned 16-bit value
00082  bit fields are shown with the lsb first
00083  */
00084 
00085 struct raw_channel
00086 {
00087   uint16_t raw_time;
00088   uint16_t dummy1;
00089   int16_t raw_data;
00090   int16_t dummy2;
00091   int16_t reserved[2];
00092   int16_t dummy3[2];
00093 };
00094 
00095 struct force_array
00096 {
00097   int16_t fx;
00098   int16_t dummy1;
00099   int16_t fy;
00100   int16_t dummy2;
00101   int16_t fz;
00102   int16_t dummy3;
00103   int16_t mx;
00104   int16_t dummy4;
00105   int16_t my;
00106   int16_t dummy5;
00107   int16_t mz;
00108   int16_t dummy6;
00109   int16_t v1;
00110   int16_t dummy7;
00111   int16_t v2;
00112   int16_t dummy8;
00113 };
00114 
00115 
00116 
00117 struct six_axis_array
00118 {
00119   int16_t fx;
00120   int16_t dummy1;
00121   int16_t fy;
00122   int16_t dummy2;
00123   int16_t fz;
00124   int16_t dummy3;
00125   int16_t mx;
00126   int16_t dummy4;
00127   int16_t my;
00128   int16_t dummy5;
00129   int16_t mz;
00130   int16_t dummy6;
00131 };
00132 
00133 struct vect_bits
00134 {
00135   uint8_t fx:1;
00136   uint8_t fy:1;
00137   uint8_t fz:1;
00138   uint8_t mx:1;
00139   uint8_t my:1;
00140   uint8_t mz:1;
00141   uint8_t changedV1:1;
00142   uint8_t changedV2:1;
00143   int16_t dummy;
00144 };
00145 
00146 struct warning_bits
00147 {
00148   uint8_t fx_near_set:1;
00149   uint8_t fy_near_set:1;
00150   uint8_t fz_near_set:1;
00151   uint8_t mx_near_set:1;
00152   uint8_t my_near_set:1;
00153   uint8_t mz_near_set:1;
00154   uint8_t reserved:8;
00155   //uint8_t        reserved : 10; // this will use 3 byte
00156   int16_t dummy;
00157 };
00158 
00159 struct error_bits
00160 {
00161   uint8_t fx_sat:1;
00162   uint8_t fy_sat:1;
00163   uint8_t fz_sat:1;
00164   uint8_t mx_sat:1;
00165   uint8_t my_sat:1;
00166   uint8_t mz_sat:1;
00167   uint8_t researved:2;
00168   //uint8_t       researved : 4; // this will use 3 byte
00169   uint8_t memry_error:1;
00170   uint8_t sensor_charge:1;
00171   uint8_t system_busy:1;
00172   uint8_t cal_crc_bad:1;
00173   uint8_t watch_dog2:1;
00174   uint8_t watch_dog:1;
00175   int16_t dummy1;
00176 };
00177 
00178 char *force_units_str[] =
00179   { (char *) "pound, inch*pound, inch*1000",
00180 (char *) "Newton, Newton*meter*10, mm*10", (char *) "kilogram-force*10, kilogram-Force*cm, mm*10",
00181 (char *) "kilopound, kiloinch*pound, inch*1000" };
00182 
00183 enum force_units
00184 {
00185   lbs_in_lbs_mils,
00186   N_dNm_mm10,
00187   kgf10_kgFcm_mm10,
00188   klbs_kin_lbs_mils,
00189   reserved_units_4,
00190   reserved_units_5,
00191   reserved_units_6,
00192   reserved_units_7
00193 };
00194 
00195 struct thresh_struct
00196 {
00197   int16_t data_address;
00198   int16_t dummy1;
00199   int16_t threshold;
00200   int16_t dummy2;
00201   int16_t bit_pattern;
00202   int16_t dummy3;
00203 };
00204 
00205 struct le_struct
00206 {
00207   int16_t latch_bits;
00208   int16_t dummy1;
00209   int16_t number_of_ge_thresholds;
00210   int16_t dummy2;
00211   int16_t number_of_le_thresholds;
00212   int16_t dummy3;
00213   thresh_struct thresholds[4];
00214   int16_t reserved;
00215   int16_t dummy4;
00216 };
00217 
00218 enum link_types
00219 {
00220   end_x_form, tx, ty, tz, rx, ry, rz, neg
00221 };
00222 
00223 struct links
00224 {
00225   link_types link_type;
00226   int16_t dummy1;
00227   int16_t link_amount;
00228   int16_t dummy2;
00229 };
00230 
00231 struct transform
00232 {
00233   links link[8];
00234 };
00235 
00236 
00237 struct force_sensor_data
00238 {
00239   raw_channel raw_channels[16]; /* offset 0x0000 */
00240   char copyright[0x18 * 4];     /* offset 0x0040 */
00241   int16_t reserved1[0x08 * 2];  /* offset 0x0058 */
00242   six_axis_array shunts;        /* offset 0x0060 */
00243   int16_t reserved2[2];         /* offset 0x0066 */
00244   int16_t dummy1[2];
00245   six_axis_array default_FS;    /* offset 0x0068 */
00246   int16_t reserved3;            /* offset 0x006e */
00247   int16_t dummy2;
00248   int16_t load_envelope_num;    /* offset 0x006f */
00249   int16_t dummy3;
00250   six_axis_array min_full_scale;        /* offset 0x0070 */
00251   int16_t reserved4;            /* offset 0x0076 */
00252   int16_t dummy4;
00253   int16_t transform_num;        /* offset 0x0077 */
00254   int16_t dummy5;
00255   six_axis_array max_full_scale;        /* offset 0x0078 */
00256   int16_t reserved5;            /* offset 0x007e */
00257   int16_t dummy6;
00258   int16_t peak_address;         /* offset 0x007f */
00259   int16_t dummy7;
00260   force_array full_scale;       /* offset 0x0080 */
00261   six_axis_array offsets;       /* offset 0x0088 */
00262   int16_t offset_num;           /* offset 0x008e */
00263   int16_t dummy8;
00264   vect_bits vect_axes;          /* offset 0x008f */
00265   force_array filter0;          /* offset 0x0090 */
00266   force_array filter1;          /* offset 0x0098 */
00267   force_array filter2;          /* offset 0x00a0 */
00268   force_array filter3;          /* offset 0x00a8 */
00269   force_array filter4;          /* offset 0x00b0 */
00270   force_array filter5;          /* offset 0x00b8 */
00271   force_array filter6;          /* offset 0x00c0 */
00272   force_array rate_data;        /* offset 0x00c8 */
00273   force_array minimum_data;     /* offset 0x00d0 */
00274   force_array maximum_data;     /* offset 0x00d8 */
00275   int16_t near_sat_value;       /* offset 0x00e0 */
00276   int16_t dummy9;
00277   int16_t sat_value;            /* offset 0x00e1 */
00278   int16_t dummy10;
00279   int16_t rate_address;         /* offset 0x00e2 */
00280   int16_t dummy11;
00281   uint16_t rate_divisor;        /* offset 0x00e3 */
00282   uint16_t dummy12;
00283   uint16_t rate_count;          /* offset 0x00e4 */
00284   uint16_t dummy13;
00285   int16_t command_word2;        /* offset 0x00e5 */
00286   int16_t dummy14;
00287   int16_t command_word1;        /* offset 0x00e6 */
00288   int16_t dummy15;
00289   int16_t command_word0;        /* offset 0x00e7 */
00290   uint16_t dummy16;
00291   uint16_t count1;              /* offset 0x00e8 */
00292   uint16_t dummy17;
00293   uint16_t count2;              /* offset 0x00e9 */
00294   uint16_t dummy18;
00295   uint16_t count3;              /* offset 0x00ea */
00296   uint16_t dummy19;
00297   uint16_t count4;              /* offset 0x00eb */
00298   uint16_t dummy20;
00299   uint16_t count5;              /* offset 0x00ec */
00300   uint16_t dummy21;
00301   uint16_t count6;              /* offset 0x00ed */
00302   uint16_t dummy22;
00303   uint16_t error_count;         /* offset 0x00ee */
00304   uint16_t dummy23;
00305   uint16_t count_x;             /* offset 0x00ef */
00306   uint16_t dummy24;
00307   warning_bits warnings;        /* offset 0x00f0 */
00308   error_bits errors;            /* offset 0x00f1 */
00309   int16_t threshold_bits;       /* offset 0x00f2 */
00310   int16_t dummy25;
00311   int16_t last_crc;             /* offset 0x00f3 */
00312   int16_t dummy26;
00313   int16_t eeprom_ver_no;        /* offset 0x00f4 */
00314   int16_t dummy27;
00315   int16_t software_ver_no;      /* offset 0x00f5 */
00316   int16_t dummy28;
00317   int16_t software_day;         /* offset 0x00f6 */
00318   int16_t dummy29;
00319   int16_t software_year;        /* offset 0x00f7 */
00320   int16_t dummy30;
00321   uint16_t serial_no;           /* offset 0x00f8 */
00322   uint16_t dummy31;
00323   uint16_t model_no;            /* offset 0x00f9 */
00324   uint16_t dummy32;
00325   int16_t cal_day;              /* offset 0x00fa */
00326   int16_t dummy33;
00327   int16_t cal_year;             /* offset 0x00fb */
00328   int16_t dummy34;
00329   force_units units;            /* offset 0x00fc */
00330   int16_t bit;                  /* offset 0x00fd */
00331   int16_t dummy35;
00332   int16_t channels;             /* offset 0x00fe */
00333   int16_t dummy36;
00334   int16_t thickness;            /* offset 0x00ff */
00335   int16_t dummy37;
00336   le_struct load_envelopes[0x10];       /* offset 0x0100 */
00337   transform transforms[0x10];   /* offset 0x0200 */
00338 };
00339 
00340 
00341 typedef struct
00342 {
00343   uint16_t msg_no;
00344   char msg_data[255];
00345 } server_msg_t;
00346 
00347 
00348 int
00349 download (unsigned int base0, unsigned int base1)
00350 {
00351   printf ("Download %x %x\n", base0, base1);
00352 
00353   int count;
00354   int index = 0;
00355   unsigned int Jr3BaseAddressH;
00356   unsigned int Jr3BaseAddressL;
00357 
00358   Jr3BaseAddressH = base0;
00359   Jr3BaseAddressL = base1;
00360 
00361   /* The first line is a line count */
00362   count = dsp[index++];
00363 
00364   /* Read in file while the count is no 0xffff */
00365   while (count != 0xffff)
00366     {
00367       int addr;
00368 
00369       /* After the count is the address */
00370       addr = dsp[index++];
00371 
00372       /* loop count times and write the data to the dsp memory */
00373       while (count > 0)
00374         {
00375           /* Check to see if this is data memory or program memory */
00376           if (addr & 0x4000)
00377             {
00378               int data = 0;
00379 
00380               /* Data memory is 16 bits and is on one line */
00381               data = dsp[index++];
00382               WriteJr3Dm (addr, data);
00383               count--;
00384               if (data != ReadJr3Dm (addr))
00385                 {
00386                   printf ("data addr: %4.4x out: %4.4x in: %4.4x\n", addr,
00387                           data, ReadJr3Dm (addr));
00388                 }
00389             }
00390           else
00391             {
00392               int data, data2;
00393               int data3;
00394 
00395 
00396               /* Program memory is 24 bits and is on two lines */
00397               data = dsp[index++];
00398               data2 = dsp[index++];
00399               WriteJr3Pm2 (addr, data, data2);
00400               count -= 2;
00401 
00402               /* Verify the write */
00403               if (((data << 8) | (data2 & 0xff)) !=
00404                   (data3 = ReadJr3Pm (addr)))
00405                 {
00406                   //      printf("pro addr: %4.4x out: %6.6x in: %6.6x\n", addr, ((data << 8)|(data2 & 0xff)), /* ReadJr3Pm(addr) */ data3);
00407                 }
00408             }
00409           addr++;
00410         }
00411       count = dsp[index++];
00412     }
00413 
00414 
00415   return 0;
00416 
00417 }
00418 
00419 void
00420 get_force_sensor_info (force_sensor_data * data, char *msg)
00421 {
00422   struct tm t1, *soft_day, *cal_day;;
00423   time_t t2;
00424   t1.tm_sec = t1.tm_min = t1.tm_hour = 0;
00425   t1.tm_isdst = -1;
00426   t1.tm_mon = 0;
00427   t1.tm_year = data->software_year - 1900;
00428   t1.tm_mday = data->software_day;
00429   t2 = mktime (&t1);
00430   soft_day = localtime (&t2);
00431   t1.tm_sec = t1.tm_min = t1.tm_hour = 0;
00432   t1.tm_isdst = -1;
00433   t1.tm_mon = 0;
00434   t1.tm_year = data->cal_year - 1900;
00435   t1.tm_mday = data->cal_day;
00436   t2 = mktime (&t1);
00437   cal_day = localtime (&t2);
00438   snprintf (msg, 128,
00439             "rom # %d, soft # %d, %d/%d/%d\n"
00440             "serial # %d, model # %d, cal day %d/%d/%d\n"
00441             "%s, %d bits, %d ch\n"
00442             "(C) %c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\n",
00443             data->eeprom_ver_no, data->software_ver_no, soft_day->tm_mday,
00444             soft_day->tm_mon + 1, soft_day->tm_year + 1900, data->serial_no,
00445             data->model_no, cal_day->tm_mday, cal_day->tm_mon + 1,
00446             cal_day->tm_year + 1900, force_units_str[1], data->bit,
00447             data->channels, data->copyright[0 * 4 + 1],
00448             data->copyright[1 * 4 + 1], data->copyright[2 * 4 + 1],
00449             data->copyright[3 * 4 + 1], data->copyright[4 * 4 + 1],
00450             data->copyright[5 * 4 + 1], data->copyright[6 * 4 + 1],
00451             data->copyright[7 * 4 + 1], data->copyright[8 * 4 + 1],
00452             data->copyright[9 * 4 + 1], data->copyright[10 * 4 + 1],
00453             data->copyright[11 * 4 + 1], data->copyright[12 * 4 + 1],
00454             data->copyright[13 * 4 + 1], data->copyright[14 * 4 + 1],
00455             data->copyright[15 * 4 + 1], data->copyright[16 * 4 + 1],
00456             data->copyright[17 * 4 + 1], data->copyright[18 * 4 + 1],
00457             data->copyright[19 * 4 + 1], data->copyright[20 * 4 + 1],
00458             data->copyright[21 * 4 + 1], data->copyright[22 * 4 + 1],
00459             data->copyright[23 * 4 + 1]);
00460 }
00461 
00462 
00463 
00464 int
00465 message_callback (message_context_t * ctp, int type, unsigned flags,
00466                   void *handle)
00467 {
00468   server_msg_t *msg;
00469   int num;
00470   char msg_reply[255];
00471 
00472   /* cast a pointer to the message data */
00473   msg = (server_msg_t *) ctp->msg;
00474 
00475   /* Print out some usefull information on the message */
00476   //printf( "\n\nServer Got Message:\n" );
00477   //printf( "  type: %d\n" , type );
00478   //printf( "  data: %s\n\n", msg->msg_data );
00479 
00480   force_sensor_data *data_0;
00481   force_sensor_data *data_1;
00482   data_0 = (force_sensor_data *) (Jr3BaseAddress0H + (Jr3DmAddrMask << 2));
00483   data_1 = (force_sensor_data *) (Jr3BaseAddress1H + (Jr3DmAddrMask << 2));
00484 
00485   /* Build the reply message */
00486   num = type - _IO_MAX;
00487   switch (num)
00488     {
00489     case 1:                     // get data
00490       {
00491         float tmp[12] = {
00492           -1.0 * (float) data_0->filter0.fx / (float) data_0->full_scale.fx,
00493           -1.0 * (float) data_0->filter0.fy / (float) data_0->full_scale.fy,
00494           -1.0 * (float) data_0->filter0.fz / (float) data_0->full_scale.fz,
00495           -1.0 * (float) data_0->filter0.mx / (float) data_0->full_scale.mx * 0.1, // Newton*meter*10
00496           -1.0 * (float) data_0->filter0.my / (float) data_0->full_scale.my * 0.1,
00497           -1.0 * (float) data_0->filter0.mz / (float) data_0->full_scale.mz * 0.1,
00498           -1.0 * (float) data_1->filter0.fx / (float) data_1->full_scale.fx,
00499           -1.0 * (float) data_1->filter0.fy / (float) data_1->full_scale.fy,
00500           -1.0 * (float) data_1->filter0.fz / (float) data_1->full_scale.fz,
00501           -1.0 * (float) data_1->filter0.mx / (float) data_1->full_scale.mx * 0.1,
00502           -1.0 * (float) data_1->filter0.my / (float) data_1->full_scale.my * 0.1,
00503           -1.0 * (float) data_1->filter0.mz / (float) data_1->full_scale.mz * 0.1
00504         };
00505         memcpy (msg_reply, tmp, sizeof (float) * 12);
00506       }
00507       break;
00508     case 2:                     // update offset
00509       data_0->offsets.fx += data_0->filter0.fx;
00510       data_0->offsets.fy += data_0->filter0.fy;
00511       data_0->offsets.fz += data_0->filter0.fz;
00512       data_0->offsets.mx += data_0->filter0.mx;
00513       data_0->offsets.my += data_0->filter0.my;
00514       data_0->offsets.mz += data_0->filter0.mz;
00515       data_1->offsets.fx += data_1->filter0.fx;
00516       data_1->offsets.fy += data_1->filter0.fy;
00517       data_1->offsets.fz += data_1->filter0.fz;
00518       data_1->offsets.mx += data_1->filter0.mx;
00519       data_1->offsets.my += data_1->filter0.my;
00520       data_1->offsets.mz += data_1->filter0.mz;
00521       break;
00522     case 3:                     // set filter
00523       break;
00524     case 4:                     // get data
00525       get_force_sensor_info (data_0, msg_reply);
00526       get_force_sensor_info (data_1, msg_reply + strlen (msg_reply));
00527       break;
00528     }
00529 
00530   /* Send a reply to the waiting (blocked) client */
00531   MsgReply (ctp->rcvid, EOK, msg_reply, 256);
00532   return 0;
00533 }
00534 
00535 
00536 
00537 int
00538 main (int argc, char **argv)
00539 {
00540   resmgr_attr_t resmgr_attr;
00541   message_attr_t message_attr;
00542   dispatch_t *dpp;
00543   dispatch_context_t *ctp, *ctp_ret;
00544   int resmgr_id, message_id;
00545 
00546   /* Create the Dispatch Interface */
00547   dpp = dispatch_create ();
00548   if (dpp == NULL)
00549     {
00550       fprintf (stderr, "dispatch_create() failed: %s\n", strerror (errno));
00551       return EXIT_FAILURE;
00552     }
00553 
00554   memset (&resmgr_attr, 0, sizeof (resmgr_attr));
00555   resmgr_attr.nparts_max = 1;
00556   resmgr_attr.msg_max_size = 2048;
00557 
00558   /* Setup the default I/O functions to handle open/read/write/... */
00559   iofunc_func_init (_RESMGR_CONNECT_NFUNCS, &ConnectFuncs,
00560                     _RESMGR_IO_NFUNCS, &IoFuncs);
00561 
00562   IoFuncs.read = io_read;
00563 
00564   /* Setup the attribute for the entry in the filesystem */
00565   iofunc_attr_init (&IoFuncAttr, S_IFNAM | 0666, 0, 0);
00566   IoFuncAttr.nbytes = strlen (buffer) + 1;      //
00567 
00568   resmgr_id = resmgr_attach (dpp, &resmgr_attr, "/dev/jr3q", _FTYPE_ANY,
00569                              0, &ConnectFuncs, &IoFuncs, &IoFuncAttr);
00570   if (resmgr_id == -1)
00571     {
00572       fprintf (stderr, "resmgr_attach() failed: %s\n", strerror (errno));
00573       return EXIT_FAILURE;
00574     }
00575 
00576   /* Setup PCI */
00577   unsigned int flags = 0;
00578   unsigned bus, function, index = 0;
00579   unsigned devid, venid;
00580   int result;
00581   unsigned lastbus, version, hardware;
00582   unsigned int address0;
00583   void *bufptr;
00584   unsigned int addr;
00585 
00586   pci_attach (flags);
00587   result = pci_present (&lastbus, &version, &hardware);
00588   if (result == -1)
00589     {
00590       printf ("PCI BIOS not present!\n");
00591     }
00592   devid = DEVICEID;
00593   venid = VENDORID;
00594   result = pci_find_device (devid, venid, index, &bus, &function);
00595   printf ("result PCI %d\n", result);
00596   printf ("bus %d\n", bus);
00597   printf ("function %d\n", function);
00598   pci_read_config_bus (bus, function, 0x10, 1, sizeof (unsigned int), bufptr);
00599 
00600   address0 = *(unsigned int *) bufptr;
00601   buffer = (char *) bufptr;
00602   printf ("Board addr 0x%x\n", *(unsigned int *) buffer);
00603 
00604   Jr3BaseAddress0H =
00605     (volatile uint32_t) mmap_device_memory (NULL, 0x100000,
00606                                             PROT_READ | PROT_WRITE |
00607                                             PROT_NOCACHE, 0, address0);
00608   Jr3BaseAddress0L =
00609     (volatile uint32_t) mmap_device_memory (NULL, 0x100000,
00610                                             PROT_READ | PROT_WRITE |
00611                                             PROT_NOCACHE, 0,
00612                                             address0 + Jr3NoAddrMask);
00613   Jr3BaseAddressH =
00614     (volatile uint32_t) mmap_device_memory (NULL, 0x100000,
00615                                             PROT_READ | PROT_WRITE |
00616                                             PROT_NOCACHE, 0, address0);
00617   WriteJr3Dm (Jr3ResetAddr, 0); // Reset DSP       
00618   wait_t ();
00619   download (Jr3BaseAddress0H, Jr3BaseAddress0L);
00620   wait_t ();
00621 
00622   Jr3BaseAddress1H =
00623     (volatile uint32_t) mmap_device_memory (NULL, 0x100000,
00624                                             PROT_READ | PROT_WRITE |
00625                                             PROT_NOCACHE, 0,
00626                                             address0 + 0x80000);
00627   Jr3BaseAddress1L =
00628     (volatile uint32_t) mmap_device_memory (NULL, 0x100000,
00629                                             PROT_READ | PROT_WRITE |
00630                                             PROT_NOCACHE, 0,
00631                                             address0 + 0x80000 +
00632                                             Jr3NoAddrMask);
00633   download (Jr3BaseAddress1H, Jr3BaseAddress1L);
00634   wait_t ();
00635 /*
00636                         Jr3BaseAddress2H = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x100000); 
00637                         Jr3BaseAddress2L = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x100000 + Jr3NoAddrMask); 
00638                         download(Jr3BaseAddress2H, Jr3BaseAddress2L);
00639                         wait_t();
00640 
00641                         Jr3BaseAddress3H = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x180000); 
00642                         Jr3BaseAddress3L = (volatile uint32_t)mmap_device_memory(NULL, 0x100000, PROT_READ|PROT_WRITE|PROT_NOCACHE, 0, address0 + 0x180000 + Jr3NoAddrMask); 
00643                         download(Jr3BaseAddress3H, Jr3BaseAddress3L);
00644                         wait_t();
00645 */
00646 
00647   //reset
00648   *(uint16_t *) (Jr3BaseAddress0H + ((0x0200 | Jr3DmAddrMask) << 2)) =
00649     (uint16_t) 0;
00650   *(uint16_t *) (Jr3BaseAddress0H + ((0x00e7 | Jr3DmAddrMask) << 2)) =
00651     (uint16_t) 0x0500;
00652 
00653 
00654   /* Setup our message callback */
00655   memset (&message_attr, 0, sizeof (message_attr));
00656   message_attr.nparts_max = 1;
00657   message_attr.msg_max_size = 4096;
00658 
00659   /* Attach a callback (handler) for two message types */
00660   message_id = message_attach (dpp, &message_attr, _IO_MAX + 1,
00661                                _IO_MAX + 10, message_callback, NULL);
00662   if (message_id == -1)
00663     {
00664       fprintf (stderr, "message_attach() failed: %s\n", strerror (errno));
00665       return EXIT_FAILURE;
00666     }
00667 
00668   /* Setup a context for the dispatch layer to use */
00669   ctp = dispatch_context_alloc (dpp);
00670   if (ctp == NULL)
00671     {
00672       fprintf (stderr, "dispatch_context_alloc() failed: %s\n",
00673                strerror (errno));
00674       return EXIT_FAILURE;
00675     }
00676 
00677   /* The "Data Pump" - get and process messages */
00678   while (1)
00679     {
00680       ctp_ret = dispatch_block (ctp);
00681       if (ctp_ret)
00682         {
00683           dispatch_handler (ctp);
00684         }
00685       else
00686         {
00687           fprintf (stderr, "dispatch_block() failed: %s\n", strerror (errno));
00688           return EXIT_FAILURE;
00689         }
00690     }
00691 
00692   return EXIT_SUCCESS;
00693 }
00694 
00695 
00696 int
00697 io_read (resmgr_context_t * ctp, io_read_t * msg, RESMGR_OCB_T * ocb)
00698 {
00699   int nleft;
00700   int nbytes;
00701   int nparts;
00702   int status;
00703 
00704   if ((status = iofunc_read_verify (ctp, msg, ocb, NULL)) != EOK)
00705     return (status);
00706 
00707   if ((msg->i.xtype & _IO_XTYPE_MASK) != _IO_XTYPE_NONE)
00708     return (ENOSYS);
00709 
00710   /*
00711    * on all reads (first and subsequent) calculate
00712    * how many bytes we can return to the client,
00713    * based upon the number of bytes available (nleft)
00714    * and the client's buffer size
00715    */
00716 
00717   nleft = ocb->attr->nbytes - ocb->offset;
00718   nbytes = min (msg->i.nbytes, nleft);
00719 
00720   if (nbytes > 0)
00721     {
00722       /* set up the return data IOV */
00723       SETIOV (ctp->iov, buffer + ocb->offset, nbytes);
00724 
00725       /* set up the number of bytes (returned by client's read()) */
00726       _IO_SET_READ_NBYTES (ctp, nbytes);
00727 
00728       /*
00729        * advance the offset by the number of bytes
00730        * returned to the client.
00731        */
00732 
00733       ocb->offset += nbytes;
00734       nparts = 1;
00735     }
00736   else
00737     {
00738       /* they've adked for zero bytes or they've already previously
00739        * read everything
00740        */
00741 
00742       _IO_SET_READ_NBYTES (ctp, 0);
00743       nparts = 0;
00744     }
00745 
00746   /* mark the access time as invalid (we just accessed it) */
00747 
00748   if (msg->i.nbytes > 0)
00749     ocb->attr->flags |= IOFUNC_ATTR_ATIME;
00750 
00751   return (_RESMGR_NPARTS (nparts));
00752 
00753 
00754 }
00755 
00756 void
00757 wait_t (void)
00758 {
00759   struct timeval tv, tv1;
00760 
00761   gettimeofday (&tv1, NULL);
00762 
00763   do
00764     {
00765       gettimeofday (&tv, NULL);
00766     }
00767   while (tv.tv_sec - tv1.tv_sec < 3);   // wait for 3 sec.
00768 
00769   return;
00770 }


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39